<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
<meta http-equiv="X-UA-Compatible" content="IE=9"/>
<meta name="generator" content="Doxygen 1.8.13"/>
<meta name="viewport" content="width=device-width, initial-scale=1"/>
<title>MRSL Motion Primitive Library: include/mpl_planner/common/env_base.h Source File</title>
<link href="tabs.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="jquery.js"></script>
<script type="text/javascript" src="dynsections.js"></script>
<link href="search/search.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="search/searchdata.js"></script>
<script type="text/javascript" src="search/search.js"></script>
<link href="doxygen.css" rel="stylesheet" type="text/css" />
</head>
<body>
<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
<div id="titlearea">
<table cellspacing="0" cellpadding="0">
 <tbody>
 <tr style="height: 56px;">
  <td id="projectalign" style="padding-left: 0.5em;">
   <div id="projectname">MRSL Motion Primitive Library
   &#160;<span id="projectnumber">1.2</span>
   </div>
   <div id="projectbrief">A motion primitive library for generating trajectory for mobile robots</div>
  </td>
 </tr>
 </tbody>
</table>
</div>
<!-- end header part -->
<!-- Generated by Doxygen 1.8.13 -->
<script type="text/javascript">
var searchBox = new SearchBox("searchBox", "search",false,'Search');
</script>
<script type="text/javascript" src="menudata.js"></script>
<script type="text/javascript" src="menu.js"></script>
<script type="text/javascript">
$(function() {
  initMenu('',true,false,'search.php','Search');
  $(document).ready(function() { init_search(); });
});
</script>
<div id="main-nav"></div>
<!-- window showing the filter options -->
<div id="MSearchSelectWindow"
     onmouseover="return searchBox.OnSearchSelectShow()"
     onmouseout="return searchBox.OnSearchSelectHide()"
     onkeydown="return searchBox.OnSearchSelectKey(event)">
</div>

<!-- iframe showing the search results (closed by default) -->
<div id="MSearchResultsWindow">
<iframe src="javascript:void(0)" frameborder="0" 
        name="MSearchResults" id="MSearchResults">
</iframe>
</div>

<div id="nav-path" class="navpath">
  <ul>
<li class="navelem"><a class="el" href="dir_d44c64559bbebec7f509842c48db8b23.html">include</a></li><li class="navelem"><a class="el" href="dir_27679d0403171f1d79ee57f30a72bf4e.html">mpl_planner</a></li><li class="navelem"><a class="el" href="dir_3dfaf0a3ea719302dece9a57e42d216e.html">common</a></li>  </ul>
</div>
</div><!-- top -->
<div class="header">
  <div class="headertitle">
<div class="title">env_base.h</div>  </div>
</div><!--header-->
<div class="contents">
<a href="env__base_8h.html">Go to the documentation of this file.</a><div class="fragment"><div class="line"><a name="l00001"></a><span class="lineno">    1</span>&#160;</div><div class="line"><a name="l00006"></a><span class="lineno">    6</span>&#160;<span class="preprocessor">#ifndef MPL_ENV_BASE_H</span></div><div class="line"><a name="l00007"></a><span class="lineno">    7</span>&#160;<span class="preprocessor">#define MPL_ENV_BASE_H</span></div><div class="line"><a name="l00008"></a><span class="lineno">    8</span>&#160;</div><div class="line"><a name="l00009"></a><span class="lineno">    9</span>&#160;<span class="preprocessor">#include &lt;<a class="code" href="trajectory_8h.html">mpl_basis/trajectory.h</a>&gt;</span></div><div class="line"><a name="l00010"></a><span class="lineno">   10</span>&#160;</div><div class="line"><a name="l00011"></a><span class="lineno">   11</span>&#160;<span class="keyword">namespace </span><a class="code" href="namespaceMPL.html">MPL</a> {</div><div class="line"><a name="l00012"></a><span class="lineno">   12</span>&#160;</div><div class="line"><a name="l00016"></a><span class="lineno">   16</span>&#160;<span class="keyword">template</span> &lt;<span class="keywordtype">int</span> Dim&gt;</div><div class="line"><a name="l00017"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html">   17</a></span>&#160;<span class="keyword">class </span><a class="code" href="classMPL_1_1env__base.html">env_base</a> {</div><div class="line"><a name="l00018"></a><span class="lineno">   18</span>&#160; <span class="keyword">public</span>:</div><div class="line"><a name="l00020"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#a73390f0939fc1efd123696f3e805f98e">   20</a></span>&#160;  <a class="code" href="classMPL_1_1env__base.html#a73390f0939fc1efd123696f3e805f98e">env_base</a>() {}</div><div class="line"><a name="l00021"></a><span class="lineno">   21</span>&#160;</div><div class="line"><a name="l00023"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#adfea242d77a013b39d94b8e5da4289fe">   23</a></span>&#160;  <span class="keyword">virtual</span> <span class="keywordtype">bool</span> <a class="code" href="classMPL_1_1env__base.html#adfea242d77a013b39d94b8e5da4289fe">is_goal</a>(<span class="keyword">const</span> <a class="code" href="structWaypoint.html">Waypoint&lt;Dim&gt;</a>&amp; state)<span class="keyword"> const </span>{</div><div class="line"><a name="l00024"></a><span class="lineno">   24</span>&#160;    <span class="keywordflow">if</span> (state.<a class="code" href="structWaypoint.html#a9661067e17763fffb65d99cabaff802a">t</a> &gt;= <a class="code" href="classMPL_1_1env__base.html#ad592f0117a0ef0ac15225855ae62c4cc">t_max_</a>) <span class="keywordflow">return</span> <span class="keyword">true</span>;</div><div class="line"><a name="l00025"></a><span class="lineno">   25</span>&#160;    <span class="keywordtype">bool</span> goaled =</div><div class="line"><a name="l00026"></a><span class="lineno">   26</span>&#160;        (state.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a> - <a class="code" href="classMPL_1_1env__base.html#a8e6706c18fa58da08174f7c0c8209312">goal_node_</a>.pos).<span class="keyword">template</span> lpNorm&lt;Eigen::Infinity&gt;() &lt;=</div><div class="line"><a name="l00027"></a><span class="lineno">   27</span>&#160;        <a class="code" href="classMPL_1_1env__base.html#ac85f1da0ec8736ba5c379bbdb6634fe2">tol_pos_</a>;</div><div class="line"><a name="l00028"></a><span class="lineno">   28</span>&#160;    <span class="keywordflow">if</span> (goaled &amp;&amp; <a class="code" href="classMPL_1_1env__base.html#a2a6216a2e0a6fcd1bf2942b145daa889">tol_vel_</a> &gt;= 0)</div><div class="line"><a name="l00029"></a><span class="lineno">   29</span>&#160;      goaled =</div><div class="line"><a name="l00030"></a><span class="lineno">   30</span>&#160;          (state.<a class="code" href="structWaypoint.html#a91393e0d8d73afb9df512710e56f02a5">vel</a> - <a class="code" href="classMPL_1_1env__base.html#a8e6706c18fa58da08174f7c0c8209312">goal_node_</a>.vel).<span class="keyword">template</span> lpNorm&lt;Eigen::Infinity&gt;() &lt;=</div><div class="line"><a name="l00031"></a><span class="lineno">   31</span>&#160;          <a class="code" href="classMPL_1_1env__base.html#a2a6216a2e0a6fcd1bf2942b145daa889">tol_vel_</a>;</div><div class="line"><a name="l00032"></a><span class="lineno">   32</span>&#160;    <span class="keywordflow">if</span> (goaled &amp;&amp; <a class="code" href="classMPL_1_1env__base.html#ac49a422f3e1b826123a12389024b9c50">tol_acc_</a> &gt;= 0)</div><div class="line"><a name="l00033"></a><span class="lineno">   33</span>&#160;      goaled =</div><div class="line"><a name="l00034"></a><span class="lineno">   34</span>&#160;          (state.<a class="code" href="structWaypoint.html#a648217ed387d4e226bec8ecb05bfefe8">acc</a> - <a class="code" href="classMPL_1_1env__base.html#a8e6706c18fa58da08174f7c0c8209312">goal_node_</a>.acc).<span class="keyword">template</span> lpNorm&lt;Eigen::Infinity&gt;() &lt;=</div><div class="line"><a name="l00035"></a><span class="lineno">   35</span>&#160;          <a class="code" href="classMPL_1_1env__base.html#ac49a422f3e1b826123a12389024b9c50">tol_acc_</a>;</div><div class="line"><a name="l00036"></a><span class="lineno">   36</span>&#160;    <span class="keywordflow">if</span> (goaled &amp;&amp; <a class="code" href="classMPL_1_1env__base.html#aee3b399510cbd3dc9690d65f0c099673">tol_yaw_</a> &gt;= 0)</div><div class="line"><a name="l00037"></a><span class="lineno">   37</span>&#160;      goaled = std::abs(state.<a class="code" href="structWaypoint.html#ad5c2f2b8f6dba96bb93faccde844e9a8">yaw</a> - <a class="code" href="classMPL_1_1env__base.html#a8e6706c18fa58da08174f7c0c8209312">goal_node_</a>.yaw) &lt;= <a class="code" href="classMPL_1_1env__base.html#aee3b399510cbd3dc9690d65f0c099673">tol_yaw_</a>;</div><div class="line"><a name="l00038"></a><span class="lineno">   38</span>&#160;    <span class="keywordflow">return</span> goaled;</div><div class="line"><a name="l00039"></a><span class="lineno">   39</span>&#160;  }</div><div class="line"><a name="l00040"></a><span class="lineno">   40</span>&#160;</div><div class="line"><a name="l00046"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#a3f5a3aa9e6be63c4bbfe96e09b56a68b">   46</a></span>&#160;  <span class="keyword">virtual</span> <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> <a class="code" href="classMPL_1_1env__base.html#a3f5a3aa9e6be63c4bbfe96e09b56a68b">get_heur</a>(<span class="keyword">const</span> <a class="code" href="structWaypoint.html">Waypoint&lt;Dim&gt;</a>&amp; state)<span class="keyword"> const </span>{</div><div class="line"><a name="l00047"></a><span class="lineno">   47</span>&#160;    <span class="keywordflow">if</span> (<a class="code" href="classMPL_1_1env__base.html#a8e6706c18fa58da08174f7c0c8209312">goal_node_</a> == state) <span class="keywordflow">return</span> 0;</div><div class="line"><a name="l00048"></a><span class="lineno">   48</span>&#160;    <span class="keywordtype">size_t</span> <span class="keywordtype">id</span> = state.<a class="code" href="structWaypoint.html#a9661067e17763fffb65d99cabaff802a">t</a> / <a class="code" href="classMPL_1_1env__base.html#a75662e4867f05ecc21cd1232d2126a11">dt_</a>;</div><div class="line"><a name="l00049"></a><span class="lineno">   49</span>&#160;    <span class="keywordflow">if</span> (!<a class="code" href="classMPL_1_1env__base.html#ad12eb1d6268bebc2542a875a3278551a">prior_traj_</a>.empty() &amp;&amp; <span class="keywordtype">id</span> &lt; <a class="code" href="classMPL_1_1env__base.html#ad12eb1d6268bebc2542a875a3278551a">prior_traj_</a>.size())</div><div class="line"><a name="l00050"></a><span class="lineno">   50</span>&#160;      <span class="keywordflow">return</span> <a class="code" href="classMPL_1_1env__base.html#a6bda580c1bc7ca88ce2671f1533fc08a">cal_heur</a>(state, <a class="code" href="classMPL_1_1env__base.html#ad12eb1d6268bebc2542a875a3278551a">prior_traj_</a>[<span class="keywordtype">id</span>].first) + <a class="code" href="classMPL_1_1env__base.html#ad12eb1d6268bebc2542a875a3278551a">prior_traj_</a>[id].second;</div><div class="line"><a name="l00051"></a><span class="lineno">   51</span>&#160;    <span class="keywordflow">else</span></div><div class="line"><a name="l00052"></a><span class="lineno">   52</span>&#160;      <span class="keywordflow">return</span> <a class="code" href="classMPL_1_1env__base.html#a6bda580c1bc7ca88ce2671f1533fc08a">cal_heur</a>(state, <a class="code" href="classMPL_1_1env__base.html#a8e6706c18fa58da08174f7c0c8209312">goal_node_</a>);</div><div class="line"><a name="l00053"></a><span class="lineno">   53</span>&#160;  }</div><div class="line"><a name="l00054"></a><span class="lineno">   54</span>&#160;</div><div class="line"><a name="l00056"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#a6bda580c1bc7ca88ce2671f1533fc08a">   56</a></span>&#160;  <span class="keyword">virtual</span> <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> <a class="code" href="classMPL_1_1env__base.html#a6bda580c1bc7ca88ce2671f1533fc08a">cal_heur</a>(<span class="keyword">const</span> <a class="code" href="structWaypoint.html">Waypoint&lt;Dim&gt;</a>&amp; state,</div><div class="line"><a name="l00057"></a><span class="lineno">   57</span>&#160;                             <span class="keyword">const</span> <a class="code" href="structWaypoint.html">Waypoint&lt;Dim&gt;</a>&amp; goal)<span class="keyword"> const </span>{</div><div class="line"><a name="l00058"></a><span class="lineno">   58</span>&#160;    <span class="keywordflow">if</span> (<a class="code" href="classMPL_1_1env__base.html#a12ccadddac2cf7cf643ce4af8982054c">heur_ignore_dynamics_</a>) {</div><div class="line"><a name="l00059"></a><span class="lineno">   59</span>&#160;      <span class="keywordflow">if</span> (<a class="code" href="classMPL_1_1env__base.html#adcd1fa1f64b3b836228d9a79f91dff0e">v_max_</a> &gt; 0) {</div><div class="line"><a name="l00060"></a><span class="lineno">   60</span>&#160;        <span class="keywordflow">return</span> <a class="code" href="classMPL_1_1env__base.html#a5c6a2e729131e36e72b51266d887ae8f">w_</a> * (state.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a> - goal.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a>).<span class="keyword">template</span> lpNorm&lt;Eigen::Infinity&gt;() /</div><div class="line"><a name="l00061"></a><span class="lineno">   61</span>&#160;               <a class="code" href="classMPL_1_1env__base.html#adcd1fa1f64b3b836228d9a79f91dff0e">v_max_</a>;</div><div class="line"><a name="l00062"></a><span class="lineno">   62</span>&#160;      } <span class="keywordflow">else</span></div><div class="line"><a name="l00063"></a><span class="lineno">   63</span>&#160;        <span class="keywordflow">return</span> <a class="code" href="classMPL_1_1env__base.html#a5c6a2e729131e36e72b51266d887ae8f">w_</a> * (state.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a> - goal.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a>).<span class="keyword">template</span> lpNorm&lt;Eigen::Infinity&gt;();</div><div class="line"><a name="l00064"></a><span class="lineno">   64</span>&#160;    }</div><div class="line"><a name="l00065"></a><span class="lineno">   65</span>&#160;    <span class="comment">// return 0;</span></div><div class="line"><a name="l00066"></a><span class="lineno">   66</span>&#160;    <span class="comment">// return w_*(state.pos - goal.pos).norm();</span></div><div class="line"><a name="l00067"></a><span class="lineno">   67</span>&#160;    <span class="keywordflow">if</span> (state.<a class="code" href="structWaypoint.html#a32d4053f0b72324f20f1d1804fe37755">control</a> == Control::JRK &amp;&amp; goal.<a class="code" href="structWaypoint.html#a32d4053f0b72324f20f1d1804fe37755">control</a> == Control::JRK) {</div><div class="line"><a name="l00068"></a><span class="lineno">   68</span>&#160;      <span class="keyword">const</span> <a class="code" href="data__type_8h.html#a3a0c45655a5e009e56634ccde0c5c575">Vecf&lt;Dim&gt;</a> dp = goal.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a> - state.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a>;</div><div class="line"><a name="l00069"></a><span class="lineno">   69</span>&#160;      <span class="keyword">const</span> <a class="code" href="data__type_8h.html#a3a0c45655a5e009e56634ccde0c5c575">Vecf&lt;Dim&gt;</a> v0 = state.<a class="code" href="structWaypoint.html#a91393e0d8d73afb9df512710e56f02a5">vel</a>;</div><div class="line"><a name="l00070"></a><span class="lineno">   70</span>&#160;      <span class="keyword">const</span> <a class="code" href="data__type_8h.html#a3a0c45655a5e009e56634ccde0c5c575">Vecf&lt;Dim&gt;</a> v1 = goal.<a class="code" href="structWaypoint.html#a91393e0d8d73afb9df512710e56f02a5">vel</a>;</div><div class="line"><a name="l00071"></a><span class="lineno">   71</span>&#160;      <span class="keyword">const</span> <a class="code" href="data__type_8h.html#a3a0c45655a5e009e56634ccde0c5c575">Vecf&lt;Dim&gt;</a> a0 = state.<a class="code" href="structWaypoint.html#a648217ed387d4e226bec8ecb05bfefe8">acc</a>;</div><div class="line"><a name="l00072"></a><span class="lineno">   72</span>&#160;      <span class="keyword">const</span> <a class="code" href="data__type_8h.html#a3a0c45655a5e009e56634ccde0c5c575">Vecf&lt;Dim&gt;</a> a1 = goal.<a class="code" href="structWaypoint.html#a648217ed387d4e226bec8ecb05bfefe8">acc</a>;</div><div class="line"><a name="l00073"></a><span class="lineno">   73</span>&#160;      <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> a = <a class="code" href="classMPL_1_1env__base.html#a5c6a2e729131e36e72b51266d887ae8f">w_</a>;</div><div class="line"><a name="l00074"></a><span class="lineno">   74</span>&#160;      <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> b = 0;</div><div class="line"><a name="l00075"></a><span class="lineno">   75</span>&#160;      <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> c = -9 * a0.dot(a0) + 6 * a0.dot(a1) - 9 * a1.dot(a1);</div><div class="line"><a name="l00076"></a><span class="lineno">   76</span>&#160;      <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> d = -144 * a0.dot(v0) - 96 * a0.dot(v1) + 96 * a1.dot(v0) +</div><div class="line"><a name="l00077"></a><span class="lineno">   77</span>&#160;                    144 * a1.dot(v1);</div><div class="line"><a name="l00078"></a><span class="lineno">   78</span>&#160;      <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> e = 360 * (a0 - a1).dot(dp) - 576 * v0.dot(v0) -</div><div class="line"><a name="l00079"></a><span class="lineno">   79</span>&#160;                    1008 * v0.dot(v1) - 576 * v1.dot(v1);</div><div class="line"><a name="l00080"></a><span class="lineno">   80</span>&#160;      <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> f = 2880 * dp.dot(v0 + v1);</div><div class="line"><a name="l00081"></a><span class="lineno">   81</span>&#160;      <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> g = -3600 * dp.dot(dp);</div><div class="line"><a name="l00082"></a><span class="lineno">   82</span>&#160;</div><div class="line"><a name="l00083"></a><span class="lineno">   83</span>&#160;      std::vector&lt;decimal_t&gt; ts = <a class="code" href="math_8h.html#a981120793ddb8cdf9ce466e8c2e70e4b">solve</a>(a, b, c, d, e, f, g);</div><div class="line"><a name="l00084"></a><span class="lineno">   84</span>&#160;</div><div class="line"><a name="l00085"></a><span class="lineno">   85</span>&#160;      <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> t_bar =</div><div class="line"><a name="l00086"></a><span class="lineno">   86</span>&#160;          (state.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a> - goal.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a>).<span class="keyword">template</span> lpNorm&lt;Eigen::Infinity&gt;() / <a class="code" href="classMPL_1_1env__base.html#adcd1fa1f64b3b836228d9a79f91dff0e">v_max_</a>;</div><div class="line"><a name="l00087"></a><span class="lineno">   87</span>&#160;      ts.push_back(t_bar);</div><div class="line"><a name="l00088"></a><span class="lineno">   88</span>&#160;      <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> min_cost = std::numeric_limits&lt;decimal_t&gt;::max();</div><div class="line"><a name="l00089"></a><span class="lineno">   89</span>&#160;      <span class="keywordflow">for</span> (<span class="keyword">auto</span> t : ts) {</div><div class="line"><a name="l00090"></a><span class="lineno">   90</span>&#160;        <span class="keywordflow">if</span> (t &lt; t_bar) <span class="keywordflow">continue</span>;</div><div class="line"><a name="l00091"></a><span class="lineno">   91</span>&#160;        <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> cost = a * t - c / t - d / 2 / t / t - e / 3 / t / t / t -</div><div class="line"><a name="l00092"></a><span class="lineno">   92</span>&#160;                         f / 4 / t / t / t / t - g / 5 / t / t / t / t / t;</div><div class="line"><a name="l00093"></a><span class="lineno">   93</span>&#160;        <span class="keywordflow">if</span> (cost &lt; min_cost) min_cost = cost;</div><div class="line"><a name="l00094"></a><span class="lineno">   94</span>&#160;      }</div><div class="line"><a name="l00095"></a><span class="lineno">   95</span>&#160;      <span class="keywordflow">return</span> min_cost;</div><div class="line"><a name="l00096"></a><span class="lineno">   96</span>&#160;    }</div><div class="line"><a name="l00097"></a><span class="lineno">   97</span>&#160;</div><div class="line"><a name="l00098"></a><span class="lineno">   98</span>&#160;    <span class="keywordflow">else</span> <span class="keywordflow">if</span> (state.<a class="code" href="structWaypoint.html#a32d4053f0b72324f20f1d1804fe37755">control</a> == Control::JRK &amp;&amp; goal.<a class="code" href="structWaypoint.html#a32d4053f0b72324f20f1d1804fe37755">control</a> == Control::ACC) {</div><div class="line"><a name="l00099"></a><span class="lineno">   99</span>&#160;      <span class="keyword">const</span> <a class="code" href="data__type_8h.html#a3a0c45655a5e009e56634ccde0c5c575">Vecf&lt;Dim&gt;</a> dp = goal.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a> - state.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a>;</div><div class="line"><a name="l00100"></a><span class="lineno">  100</span>&#160;      <span class="keyword">const</span> <a class="code" href="data__type_8h.html#a3a0c45655a5e009e56634ccde0c5c575">Vecf&lt;Dim&gt;</a> v0 = state.<a class="code" href="structWaypoint.html#a91393e0d8d73afb9df512710e56f02a5">vel</a>;</div><div class="line"><a name="l00101"></a><span class="lineno">  101</span>&#160;      <span class="keyword">const</span> <a class="code" href="data__type_8h.html#a3a0c45655a5e009e56634ccde0c5c575">Vecf&lt;Dim&gt;</a> v1 = goal.<a class="code" href="structWaypoint.html#a91393e0d8d73afb9df512710e56f02a5">vel</a>;</div><div class="line"><a name="l00102"></a><span class="lineno">  102</span>&#160;      <span class="keyword">const</span> <a class="code" href="data__type_8h.html#a3a0c45655a5e009e56634ccde0c5c575">Vecf&lt;Dim&gt;</a> a0 = state.<a class="code" href="structWaypoint.html#a648217ed387d4e226bec8ecb05bfefe8">acc</a>;</div><div class="line"><a name="l00103"></a><span class="lineno">  103</span>&#160;</div><div class="line"><a name="l00104"></a><span class="lineno">  104</span>&#160;      <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> a = <a class="code" href="classMPL_1_1env__base.html#a5c6a2e729131e36e72b51266d887ae8f">w_</a>;</div><div class="line"><a name="l00105"></a><span class="lineno">  105</span>&#160;      <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> b = 0;</div><div class="line"><a name="l00106"></a><span class="lineno">  106</span>&#160;      <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> c = -8 * a0.dot(a0);</div><div class="line"><a name="l00107"></a><span class="lineno">  107</span>&#160;      <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> d = -112 * a0.dot(v0) - 48 * a0.dot(v1);</div><div class="line"><a name="l00108"></a><span class="lineno">  108</span>&#160;      <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> e = 240 * a0.dot(dp) - 384 * v0.dot(v0) - 432 * v0.dot(v1) -</div><div class="line"><a name="l00109"></a><span class="lineno">  109</span>&#160;                    144 * v1.dot(v1);</div><div class="line"><a name="l00110"></a><span class="lineno">  110</span>&#160;      <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> f = dp.dot(1600 * v0 + 960 * v1);</div><div class="line"><a name="l00111"></a><span class="lineno">  111</span>&#160;      <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> g = -1600 * dp.dot(dp);</div><div class="line"><a name="l00112"></a><span class="lineno">  112</span>&#160;</div><div class="line"><a name="l00113"></a><span class="lineno">  113</span>&#160;      std::vector&lt;decimal_t&gt; ts = <a class="code" href="math_8h.html#a981120793ddb8cdf9ce466e8c2e70e4b">solve</a>(a, b, c, d, e, f, g);</div><div class="line"><a name="l00114"></a><span class="lineno">  114</span>&#160;</div><div class="line"><a name="l00115"></a><span class="lineno">  115</span>&#160;      <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> t_bar =</div><div class="line"><a name="l00116"></a><span class="lineno">  116</span>&#160;          (state.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a> - goal.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a>).<span class="keyword">template</span> lpNorm&lt;Eigen::Infinity&gt;() / <a class="code" href="classMPL_1_1env__base.html#adcd1fa1f64b3b836228d9a79f91dff0e">v_max_</a>;</div><div class="line"><a name="l00117"></a><span class="lineno">  117</span>&#160;      ts.push_back(t_bar);</div><div class="line"><a name="l00118"></a><span class="lineno">  118</span>&#160;      <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> min_cost = std::numeric_limits&lt;decimal_t&gt;::max();</div><div class="line"><a name="l00119"></a><span class="lineno">  119</span>&#160;      <span class="keywordflow">for</span> (<span class="keyword">auto</span> t : ts) {</div><div class="line"><a name="l00120"></a><span class="lineno">  120</span>&#160;        <span class="keywordflow">if</span> (t &lt; t_bar) <span class="keywordflow">continue</span>;</div><div class="line"><a name="l00121"></a><span class="lineno">  121</span>&#160;        <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> cost = a * t - c / t - d / 2 / t / t - e / 3 / t / t / t -</div><div class="line"><a name="l00122"></a><span class="lineno">  122</span>&#160;                         f / 4 / t / t / t / t - g / 5 / t / t / t / t / t;</div><div class="line"><a name="l00123"></a><span class="lineno">  123</span>&#160;        <span class="keywordflow">if</span> (cost &lt; min_cost) min_cost = cost;</div><div class="line"><a name="l00124"></a><span class="lineno">  124</span>&#160;        <span class="comment">// printf(&quot;t: %f, cost: %f\n&quot;,t, cost);</span></div><div class="line"><a name="l00125"></a><span class="lineno">  125</span>&#160;      }</div><div class="line"><a name="l00126"></a><span class="lineno">  126</span>&#160;      <span class="keywordflow">return</span> min_cost;</div><div class="line"><a name="l00127"></a><span class="lineno">  127</span>&#160;    }</div><div class="line"><a name="l00128"></a><span class="lineno">  128</span>&#160;</div><div class="line"><a name="l00129"></a><span class="lineno">  129</span>&#160;    <span class="keywordflow">else</span> <span class="keywordflow">if</span> (state.<a class="code" href="structWaypoint.html#a32d4053f0b72324f20f1d1804fe37755">control</a> == Control::JRK &amp;&amp; goal.<a class="code" href="structWaypoint.html#a32d4053f0b72324f20f1d1804fe37755">control</a> == Control::VEL) {</div><div class="line"><a name="l00130"></a><span class="lineno">  130</span>&#160;      <span class="keyword">const</span> <a class="code" href="data__type_8h.html#a3a0c45655a5e009e56634ccde0c5c575">Vecf&lt;Dim&gt;</a> dp = goal.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a> - state.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a>;</div><div class="line"><a name="l00131"></a><span class="lineno">  131</span>&#160;      <span class="keyword">const</span> <a class="code" href="data__type_8h.html#a3a0c45655a5e009e56634ccde0c5c575">Vecf&lt;Dim&gt;</a> v0 = state.<a class="code" href="structWaypoint.html#a91393e0d8d73afb9df512710e56f02a5">vel</a>;</div><div class="line"><a name="l00132"></a><span class="lineno">  132</span>&#160;      <span class="keyword">const</span> <a class="code" href="data__type_8h.html#a3a0c45655a5e009e56634ccde0c5c575">Vecf&lt;Dim&gt;</a> a0 = state.<a class="code" href="structWaypoint.html#a648217ed387d4e226bec8ecb05bfefe8">acc</a>;</div><div class="line"><a name="l00133"></a><span class="lineno">  133</span>&#160;</div><div class="line"><a name="l00134"></a><span class="lineno">  134</span>&#160;      <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> a = <a class="code" href="classMPL_1_1env__base.html#a5c6a2e729131e36e72b51266d887ae8f">w_</a>;</div><div class="line"><a name="l00135"></a><span class="lineno">  135</span>&#160;      <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> b = 0;</div><div class="line"><a name="l00136"></a><span class="lineno">  136</span>&#160;      <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> c = -5 * a0.dot(a0);</div><div class="line"><a name="l00137"></a><span class="lineno">  137</span>&#160;      <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> d = -40 * a0.dot(v0);</div><div class="line"><a name="l00138"></a><span class="lineno">  138</span>&#160;      <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> e = 60 * a0.dot(dp) - 60 * v0.dot(v0);</div><div class="line"><a name="l00139"></a><span class="lineno">  139</span>&#160;      <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> f = 160 * dp.dot(v0);</div><div class="line"><a name="l00140"></a><span class="lineno">  140</span>&#160;      <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> g = -100 * dp.dot(dp);</div><div class="line"><a name="l00141"></a><span class="lineno">  141</span>&#160;</div><div class="line"><a name="l00142"></a><span class="lineno">  142</span>&#160;      std::vector&lt;decimal_t&gt; ts = <a class="code" href="math_8h.html#a981120793ddb8cdf9ce466e8c2e70e4b">solve</a>(a, b, c, d, e, f, g);</div><div class="line"><a name="l00143"></a><span class="lineno">  143</span>&#160;</div><div class="line"><a name="l00144"></a><span class="lineno">  144</span>&#160;      <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> t_bar =</div><div class="line"><a name="l00145"></a><span class="lineno">  145</span>&#160;          (state.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a> - goal.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a>).<span class="keyword">template</span> lpNorm&lt;Eigen::Infinity&gt;() / <a class="code" href="classMPL_1_1env__base.html#adcd1fa1f64b3b836228d9a79f91dff0e">v_max_</a>;</div><div class="line"><a name="l00146"></a><span class="lineno">  146</span>&#160;      ts.push_back(t_bar);</div><div class="line"><a name="l00147"></a><span class="lineno">  147</span>&#160;</div><div class="line"><a name="l00148"></a><span class="lineno">  148</span>&#160;      <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> min_cost = std::numeric_limits&lt;decimal_t&gt;::max();</div><div class="line"><a name="l00149"></a><span class="lineno">  149</span>&#160;      <span class="keywordflow">for</span> (<span class="keyword">auto</span> t : ts) {</div><div class="line"><a name="l00150"></a><span class="lineno">  150</span>&#160;        <span class="keywordflow">if</span> (t &lt; t_bar) <span class="keywordflow">continue</span>;</div><div class="line"><a name="l00151"></a><span class="lineno">  151</span>&#160;        <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> cost = a * t - c / t - d / 2 / t / t - e / 3 / t / t / t -</div><div class="line"><a name="l00152"></a><span class="lineno">  152</span>&#160;                         f / 4 / t / t / t / t - g / 5 / t / t / t / t / t;</div><div class="line"><a name="l00153"></a><span class="lineno">  153</span>&#160;        <span class="keywordflow">if</span> (cost &lt; min_cost) min_cost = cost;</div><div class="line"><a name="l00154"></a><span class="lineno">  154</span>&#160;      }</div><div class="line"><a name="l00155"></a><span class="lineno">  155</span>&#160;      <span class="keywordflow">return</span> min_cost;</div><div class="line"><a name="l00156"></a><span class="lineno">  156</span>&#160;    }</div><div class="line"><a name="l00157"></a><span class="lineno">  157</span>&#160;</div><div class="line"><a name="l00158"></a><span class="lineno">  158</span>&#160;    <span class="keywordflow">else</span> <span class="keywordflow">if</span> (state.<a class="code" href="structWaypoint.html#a32d4053f0b72324f20f1d1804fe37755">control</a> == Control::ACC &amp;&amp; goal.<a class="code" href="structWaypoint.html#a32d4053f0b72324f20f1d1804fe37755">control</a> == Control::ACC) {</div><div class="line"><a name="l00159"></a><span class="lineno">  159</span>&#160;      <span class="keyword">const</span> <a class="code" href="data__type_8h.html#a3a0c45655a5e009e56634ccde0c5c575">Vecf&lt;Dim&gt;</a> dp = goal.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a> - state.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a>;</div><div class="line"><a name="l00160"></a><span class="lineno">  160</span>&#160;      <span class="keyword">const</span> <a class="code" href="data__type_8h.html#a3a0c45655a5e009e56634ccde0c5c575">Vecf&lt;Dim&gt;</a> v0 = state.<a class="code" href="structWaypoint.html#a91393e0d8d73afb9df512710e56f02a5">vel</a>;</div><div class="line"><a name="l00161"></a><span class="lineno">  161</span>&#160;      <span class="keyword">const</span> <a class="code" href="data__type_8h.html#a3a0c45655a5e009e56634ccde0c5c575">Vecf&lt;Dim&gt;</a> v1 = goal.<a class="code" href="structWaypoint.html#a91393e0d8d73afb9df512710e56f02a5">vel</a>;</div><div class="line"><a name="l00162"></a><span class="lineno">  162</span>&#160;</div><div class="line"><a name="l00163"></a><span class="lineno">  163</span>&#160;      <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> c1 = -36 * dp.dot(dp);</div><div class="line"><a name="l00164"></a><span class="lineno">  164</span>&#160;      <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> c2 = 24 * (v0 + v1).dot(dp);</div><div class="line"><a name="l00165"></a><span class="lineno">  165</span>&#160;      <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> c3 = -4 * (v0.dot(v0) + v0.dot(v1) + v1.dot(v1));</div><div class="line"><a name="l00166"></a><span class="lineno">  166</span>&#160;      <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> c4 = 0;</div><div class="line"><a name="l00167"></a><span class="lineno">  167</span>&#160;      <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> c5 = <a class="code" href="classMPL_1_1env__base.html#a5c6a2e729131e36e72b51266d887ae8f">w_</a>;</div><div class="line"><a name="l00168"></a><span class="lineno">  168</span>&#160;</div><div class="line"><a name="l00169"></a><span class="lineno">  169</span>&#160;      std::vector&lt;decimal_t&gt; ts = <a class="code" href="math_8h.html#a11e53038927f416e637df83d4a0b18bc">quartic</a>(c5, c4, c3, c2, c1);</div><div class="line"><a name="l00170"></a><span class="lineno">  170</span>&#160;      <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> t_bar =</div><div class="line"><a name="l00171"></a><span class="lineno">  171</span>&#160;          (state.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a> - goal.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a>).<span class="keyword">template</span> lpNorm&lt;Eigen::Infinity&gt;() / <a class="code" href="classMPL_1_1env__base.html#adcd1fa1f64b3b836228d9a79f91dff0e">v_max_</a>;</div><div class="line"><a name="l00172"></a><span class="lineno">  172</span>&#160;      ts.push_back(t_bar);</div><div class="line"><a name="l00173"></a><span class="lineno">  173</span>&#160;</div><div class="line"><a name="l00174"></a><span class="lineno">  174</span>&#160;      <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> cost = std::numeric_limits&lt;decimal_t&gt;::max();</div><div class="line"><a name="l00175"></a><span class="lineno">  175</span>&#160;      <span class="keywordflow">for</span> (<span class="keyword">auto</span> t : ts) {</div><div class="line"><a name="l00176"></a><span class="lineno">  176</span>&#160;        <span class="keywordflow">if</span> (t &lt; t_bar) <span class="keywordflow">continue</span>;</div><div class="line"><a name="l00177"></a><span class="lineno">  177</span>&#160;        <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> c = -c1 / 3 / t / t / t - c2 / 2 / t / t - c3 / t + <a class="code" href="classMPL_1_1env__base.html#a5c6a2e729131e36e72b51266d887ae8f">w_</a> * t;</div><div class="line"><a name="l00178"></a><span class="lineno">  178</span>&#160;        <span class="keywordflow">if</span> (c &lt; cost) cost = c;</div><div class="line"><a name="l00179"></a><span class="lineno">  179</span>&#160;      }</div><div class="line"><a name="l00180"></a><span class="lineno">  180</span>&#160;</div><div class="line"><a name="l00181"></a><span class="lineno">  181</span>&#160;      <span class="keywordflow">return</span> cost;</div><div class="line"><a name="l00182"></a><span class="lineno">  182</span>&#160;    }</div><div class="line"><a name="l00183"></a><span class="lineno">  183</span>&#160;</div><div class="line"><a name="l00184"></a><span class="lineno">  184</span>&#160;    <span class="keywordflow">else</span> <span class="keywordflow">if</span> (state.<a class="code" href="structWaypoint.html#a32d4053f0b72324f20f1d1804fe37755">control</a> == Control::ACC &amp;&amp; goal.<a class="code" href="structWaypoint.html#a32d4053f0b72324f20f1d1804fe37755">control</a> == Control::VEL) {</div><div class="line"><a name="l00185"></a><span class="lineno">  185</span>&#160;      <span class="keyword">const</span> <a class="code" href="data__type_8h.html#a3a0c45655a5e009e56634ccde0c5c575">Vecf&lt;Dim&gt;</a> dp = goal.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a> - state.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a>;</div><div class="line"><a name="l00186"></a><span class="lineno">  186</span>&#160;      <span class="keyword">const</span> <a class="code" href="data__type_8h.html#a3a0c45655a5e009e56634ccde0c5c575">Vecf&lt;Dim&gt;</a> v0 = state.<a class="code" href="structWaypoint.html#a91393e0d8d73afb9df512710e56f02a5">vel</a>;</div><div class="line"><a name="l00187"></a><span class="lineno">  187</span>&#160;</div><div class="line"><a name="l00188"></a><span class="lineno">  188</span>&#160;      <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> c1 = -9 * dp.dot(dp);</div><div class="line"><a name="l00189"></a><span class="lineno">  189</span>&#160;      <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> c2 = 12 * v0.dot(dp);</div><div class="line"><a name="l00190"></a><span class="lineno">  190</span>&#160;      <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> c3 = -3 * v0.dot(v0);</div><div class="line"><a name="l00191"></a><span class="lineno">  191</span>&#160;      <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> c4 = 0;</div><div class="line"><a name="l00192"></a><span class="lineno">  192</span>&#160;      <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> c5 = <a class="code" href="classMPL_1_1env__base.html#a5c6a2e729131e36e72b51266d887ae8f">w_</a>;</div><div class="line"><a name="l00193"></a><span class="lineno">  193</span>&#160;</div><div class="line"><a name="l00194"></a><span class="lineno">  194</span>&#160;      std::vector&lt;decimal_t&gt; ts = <a class="code" href="math_8h.html#a11e53038927f416e637df83d4a0b18bc">quartic</a>(c5, c4, c3, c2, c1);</div><div class="line"><a name="l00195"></a><span class="lineno">  195</span>&#160;      <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> t_bar =</div><div class="line"><a name="l00196"></a><span class="lineno">  196</span>&#160;          (state.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a> - goal.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a>).<span class="keyword">template</span> lpNorm&lt;Eigen::Infinity&gt;() / <a class="code" href="classMPL_1_1env__base.html#adcd1fa1f64b3b836228d9a79f91dff0e">v_max_</a>;</div><div class="line"><a name="l00197"></a><span class="lineno">  197</span>&#160;      ts.push_back(t_bar);</div><div class="line"><a name="l00198"></a><span class="lineno">  198</span>&#160;</div><div class="line"><a name="l00199"></a><span class="lineno">  199</span>&#160;      <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> cost = std::numeric_limits&lt;decimal_t&gt;::max();</div><div class="line"><a name="l00200"></a><span class="lineno">  200</span>&#160;      <span class="keywordflow">for</span> (<span class="keyword">auto</span> t : ts) {</div><div class="line"><a name="l00201"></a><span class="lineno">  201</span>&#160;        <span class="keywordflow">if</span> (t &lt; t_bar) <span class="keywordflow">continue</span>;</div><div class="line"><a name="l00202"></a><span class="lineno">  202</span>&#160;        <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> c = -c1 / 3 / t / t / t - c2 / 2 / t / t - c3 / t + <a class="code" href="classMPL_1_1env__base.html#a5c6a2e729131e36e72b51266d887ae8f">w_</a> * t;</div><div class="line"><a name="l00203"></a><span class="lineno">  203</span>&#160;        <span class="keywordflow">if</span> (c &lt; cost) cost = c;</div><div class="line"><a name="l00204"></a><span class="lineno">  204</span>&#160;      }</div><div class="line"><a name="l00205"></a><span class="lineno">  205</span>&#160;</div><div class="line"><a name="l00206"></a><span class="lineno">  206</span>&#160;      <span class="keywordflow">return</span> cost;</div><div class="line"><a name="l00207"></a><span class="lineno">  207</span>&#160;    } <span class="keywordflow">else</span> <span class="keywordflow">if</span> (state.<a class="code" href="structWaypoint.html#a32d4053f0b72324f20f1d1804fe37755">control</a> == Control::VEL &amp;&amp; goal.<a class="code" href="structWaypoint.html#a32d4053f0b72324f20f1d1804fe37755">control</a> == Control::VEL)</div><div class="line"><a name="l00208"></a><span class="lineno">  208</span>&#160;      <span class="keywordflow">return</span> (<a class="code" href="classMPL_1_1env__base.html#a5c6a2e729131e36e72b51266d887ae8f">w_</a> + 1) * (state.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a> - goal.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a>).norm();</div><div class="line"><a name="l00209"></a><span class="lineno">  209</span>&#160;    <span class="keywordflow">else</span></div><div class="line"><a name="l00210"></a><span class="lineno">  210</span>&#160;      <span class="keywordflow">return</span> <a class="code" href="classMPL_1_1env__base.html#a5c6a2e729131e36e72b51266d887ae8f">w_</a> * (state.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a> - goal.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a>).norm() / <a class="code" href="classMPL_1_1env__base.html#adcd1fa1f64b3b836228d9a79f91dff0e">v_max_</a>;</div><div class="line"><a name="l00211"></a><span class="lineno">  211</span>&#160;  }</div><div class="line"><a name="l00212"></a><span class="lineno">  212</span>&#160;</div><div class="line"><a name="l00214"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#aa9a38f44386ae7778411212b63fa42b7">  214</a></span>&#160;  <span class="keyword">inline</span> <a class="code" href="data__type_8h.html#ac208833a6aa7ec29f39eb5ae142aaede">Veci&lt;Dim&gt;</a> <a class="code" href="classMPL_1_1env__base.html#aa9a38f44386ae7778411212b63fa42b7">round</a>(<span class="keyword">const</span> <a class="code" href="data__type_8h.html#a3a0c45655a5e009e56634ccde0c5c575">Vecf&lt;Dim&gt;</a>&amp; vec, <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> res)<span class="keyword"> const </span>{</div><div class="line"><a name="l00215"></a><span class="lineno">  215</span>&#160;    <a class="code" href="data__type_8h.html#ac208833a6aa7ec29f39eb5ae142aaede">Veci&lt;Dim&gt;</a> vecI;</div><div class="line"><a name="l00216"></a><span class="lineno">  216</span>&#160;    <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt; Dim; i++) vecI(i) = std::round(vec(i) / res);</div><div class="line"><a name="l00217"></a><span class="lineno">  217</span>&#160;    <span class="keywordflow">return</span> vecI;</div><div class="line"><a name="l00218"></a><span class="lineno">  218</span>&#160;  }</div><div class="line"><a name="l00219"></a><span class="lineno">  219</span>&#160;</div><div class="line"><a name="l00221"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#aa929ae0eb7125962b07a3fcff970251d">  221</a></span>&#160;  std::string <a class="code" href="classMPL_1_1env__base.html#aa929ae0eb7125962b07a3fcff970251d">to_string</a>(<span class="keyword">const</span> <a class="code" href="data__type_8h.html#ac208833a6aa7ec29f39eb5ae142aaede">Veci&lt;Dim&gt;</a>&amp; vec)<span class="keyword"> const </span>{</div><div class="line"><a name="l00222"></a><span class="lineno">  222</span>&#160;    std::string str;</div><div class="line"><a name="l00223"></a><span class="lineno">  223</span>&#160;    <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt; Dim; i++) str += std::to_string(vec(i)) + <span class="stringliteral">&quot;-&quot;</span>;</div><div class="line"><a name="l00224"></a><span class="lineno">  224</span>&#160;    <span class="keywordflow">return</span> str;</div><div class="line"><a name="l00225"></a><span class="lineno">  225</span>&#160;  }</div><div class="line"><a name="l00226"></a><span class="lineno">  226</span>&#160;</div><div class="line"><a name="l00228"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#a22bf748adf6a4febbc7b4b943e76940a">  228</a></span>&#160;  <span class="keywordtype">void</span> <a class="code" href="classMPL_1_1env__base.html#a22bf748adf6a4febbc7b4b943e76940a">forward_action</a>(<span class="keyword">const</span> <a class="code" href="structWaypoint.html">Waypoint&lt;Dim&gt;</a>&amp; curr, <span class="keywordtype">int</span> action_id,</div><div class="line"><a name="l00229"></a><span class="lineno">  229</span>&#160;                      <a class="code" href="classPrimitive.html">Primitive&lt;Dim&gt;</a>&amp; pr)<span class="keyword"> const </span>{</div><div class="line"><a name="l00230"></a><span class="lineno">  230</span>&#160;    pr = <a class="code" href="classPrimitive.html">Primitive&lt;Dim&gt;</a>(curr, <a class="code" href="classMPL_1_1env__base.html#a632147ec3e706f2bf10fb196c9d0a38c">U_</a>[action_id], <a class="code" href="classMPL_1_1env__base.html#a75662e4867f05ecc21cd1232d2126a11">dt_</a>);</div><div class="line"><a name="l00231"></a><span class="lineno">  231</span>&#160;  }</div><div class="line"><a name="l00232"></a><span class="lineno">  232</span>&#160;</div><div class="line"><a name="l00234"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#a5fe3e7b63d589bbf1ccec75cb28e6626">  234</a></span>&#160;  <span class="keywordtype">void</span> <a class="code" href="classMPL_1_1env__base.html#a5fe3e7b63d589bbf1ccec75cb28e6626">set_u</a>(<span class="keyword">const</span> <a class="code" href="data__type_8h.html#a6b9210d48a0a541940af71827aad1b41">vec_E&lt;VecDf&gt;</a>&amp; U) { <a class="code" href="classMPL_1_1env__base.html#a632147ec3e706f2bf10fb196c9d0a38c">U_</a> = U; }</div><div class="line"><a name="l00235"></a><span class="lineno">  235</span>&#160;</div><div class="line"><a name="l00237"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#ab2b77b8f234481dc74d0223a87dcff3c">  237</a></span>&#160;  <span class="keywordtype">void</span> <a class="code" href="classMPL_1_1env__base.html#ab2b77b8f234481dc74d0223a87dcff3c">set_v_max</a>(<a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> v) { <a class="code" href="classMPL_1_1env__base.html#adcd1fa1f64b3b836228d9a79f91dff0e">v_max_</a> = v; }</div><div class="line"><a name="l00238"></a><span class="lineno">  238</span>&#160;</div><div class="line"><a name="l00240"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#a17295278edfcd6cbc265d01371aac43f">  240</a></span>&#160;  <span class="keywordtype">void</span> <a class="code" href="classMPL_1_1env__base.html#a17295278edfcd6cbc265d01371aac43f">set_a_max</a>(<a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> a) { <a class="code" href="classMPL_1_1env__base.html#a7be6859be90f54ef175482d0aa6020c9">a_max_</a> = a; }</div><div class="line"><a name="l00241"></a><span class="lineno">  241</span>&#160;</div><div class="line"><a name="l00243"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#a70b6fa02c57c146aa26eb85abc085d2a">  243</a></span>&#160;  <span class="keywordtype">void</span> <a class="code" href="classMPL_1_1env__base.html#a70b6fa02c57c146aa26eb85abc085d2a">set_j_max</a>(<a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> j) { <a class="code" href="classMPL_1_1env__base.html#a6a67a6409ed46cd0c04f095a18996178">j_max_</a> = j; }</div><div class="line"><a name="l00244"></a><span class="lineno">  244</span>&#160;</div><div class="line"><a name="l00246"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#aaff1cc4715bc47e32ef95d82a68d47d1">  246</a></span>&#160;  <span class="keywordtype">void</span> <a class="code" href="classMPL_1_1env__base.html#aaff1cc4715bc47e32ef95d82a68d47d1">set_yaw_max</a>(<a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> yaw) { <a class="code" href="classMPL_1_1env__base.html#a6d1787397e20de82495fb9c381c14980">yaw_max_</a> = yaw; }</div><div class="line"><a name="l00247"></a><span class="lineno">  247</span>&#160;</div><div class="line"><a name="l00249"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#a84764f976374a3964c5299ce837fa762">  249</a></span>&#160;  <span class="keyword">virtual</span> <span class="keywordtype">void</span> <a class="code" href="classMPL_1_1env__base.html#a84764f976374a3964c5299ce837fa762">set_prior_trajectory</a>(<span class="keyword">const</span> <a class="code" href="classTrajectory.html">Trajectory&lt;Dim&gt;</a>&amp; traj) {</div><div class="line"><a name="l00250"></a><span class="lineno">  250</span>&#160;    <a class="code" href="classMPL_1_1env__base.html#ad12eb1d6268bebc2542a875a3278551a">prior_traj_</a>.clear();</div><div class="line"><a name="l00251"></a><span class="lineno">  251</span>&#160;    <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> total_time = traj.<a class="code" href="classTrajectory.html#a93f4d5f86feb5f56e5fae7fd02311e9c">getTotalTime</a>();</div><div class="line"><a name="l00252"></a><span class="lineno">  252</span>&#160;    <span class="keywordflow">for</span> (<a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> t = 0; t &lt; total_time; t += <a class="code" href="classMPL_1_1env__base.html#a75662e4867f05ecc21cd1232d2126a11">dt_</a>) {</div><div class="line"><a name="l00253"></a><span class="lineno">  253</span>&#160;      <a class="code" href="classMPL_1_1env__base.html#ad12eb1d6268bebc2542a875a3278551a">prior_traj_</a>.push_back(</div><div class="line"><a name="l00254"></a><span class="lineno">  254</span>&#160;          std::make_pair(traj.<a class="code" href="classTrajectory.html#aec62c9d9b1a35202df2072ea3efcef09">evaluate</a>(t), <a class="code" href="classMPL_1_1env__base.html#a5c6a2e729131e36e72b51266d887ae8f">w_</a> * (total_time - t)));</div><div class="line"><a name="l00255"></a><span class="lineno">  255</span>&#160;    }</div><div class="line"><a name="l00256"></a><span class="lineno">  256</span>&#160;  }</div><div class="line"><a name="l00257"></a><span class="lineno">  257</span>&#160;</div><div class="line"><a name="l00259"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#af8408acf8e66abd1d07f69d55e03a4dc">  259</a></span>&#160;  <span class="keywordtype">void</span> <a class="code" href="classMPL_1_1env__base.html#af8408acf8e66abd1d07f69d55e03a4dc">set_dt</a>(<a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> dt) { <a class="code" href="classMPL_1_1env__base.html#a75662e4867f05ecc21cd1232d2126a11">dt_</a> = dt; }</div><div class="line"><a name="l00260"></a><span class="lineno">  260</span>&#160;</div><div class="line"><a name="l00262"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#a8c3490b413cb9a37dd40c8e250763072">  262</a></span>&#160;  <span class="keywordtype">void</span> <a class="code" href="classMPL_1_1env__base.html#a8c3490b413cb9a37dd40c8e250763072">set_tol_pos</a>(<a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> pos) { <a class="code" href="classMPL_1_1env__base.html#ac85f1da0ec8736ba5c379bbdb6634fe2">tol_pos_</a> = pos; }</div><div class="line"><a name="l00263"></a><span class="lineno">  263</span>&#160;</div><div class="line"><a name="l00265"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#a868d8d096eff02ddc71dc7163271e671">  265</a></span>&#160;  <span class="keywordtype">void</span> <a class="code" href="classMPL_1_1env__base.html#a868d8d096eff02ddc71dc7163271e671">set_tol_vel</a>(<a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> vel) { <a class="code" href="classMPL_1_1env__base.html#a2a6216a2e0a6fcd1bf2942b145daa889">tol_vel_</a> = vel; }</div><div class="line"><a name="l00266"></a><span class="lineno">  266</span>&#160;</div><div class="line"><a name="l00268"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#a9d1c12973cfed2b1ceea81664008ed55">  268</a></span>&#160;  <span class="keywordtype">void</span> <a class="code" href="classMPL_1_1env__base.html#a9d1c12973cfed2b1ceea81664008ed55">set_tol_acc</a>(<a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> acc) { <a class="code" href="classMPL_1_1env__base.html#ac49a422f3e1b826123a12389024b9c50">tol_acc_</a> = acc; }</div><div class="line"><a name="l00269"></a><span class="lineno">  269</span>&#160;</div><div class="line"><a name="l00271"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#aa2209dde7b5a71de3d4992232203d29a">  271</a></span>&#160;  <span class="keywordtype">void</span> <a class="code" href="classMPL_1_1env__base.html#aa2209dde7b5a71de3d4992232203d29a">set_tol_yaw</a>(<a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> yaw) { <a class="code" href="classMPL_1_1env__base.html#aee3b399510cbd3dc9690d65f0c099673">tol_yaw_</a> = yaw; }</div><div class="line"><a name="l00272"></a><span class="lineno">  272</span>&#160;</div><div class="line"><a name="l00274"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#a78ffc9bb089726cf9813d57cfef43d03">  274</a></span>&#160;  <span class="keywordtype">void</span> <a class="code" href="classMPL_1_1env__base.html#a78ffc9bb089726cf9813d57cfef43d03">set_w</a>(<a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> w) { <a class="code" href="classMPL_1_1env__base.html#a5c6a2e729131e36e72b51266d887ae8f">w_</a> = w; }</div><div class="line"><a name="l00275"></a><span class="lineno">  275</span>&#160;</div><div class="line"><a name="l00277"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#a9cb1a2bea89036d66138005f2c90c903">  277</a></span>&#160;  <span class="keywordtype">void</span> <a class="code" href="classMPL_1_1env__base.html#a9cb1a2bea89036d66138005f2c90c903">set_wyaw</a>(<a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> wyaw) { <a class="code" href="classMPL_1_1env__base.html#ad47343816c81f60e207cd424a957c491">wyaw_</a> = wyaw; }</div><div class="line"><a name="l00278"></a><span class="lineno">  278</span>&#160;</div><div class="line"><a name="l00280"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#ae01a34b11afd9c21168d04a009f12248">  280</a></span>&#160;  <span class="keyword">virtual</span> <span class="keywordtype">void</span> <a class="code" href="classMPL_1_1env__base.html#ae01a34b11afd9c21168d04a009f12248">set_potential_weight</a>(<a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> w) {}</div><div class="line"><a name="l00281"></a><span class="lineno">  281</span>&#160;</div><div class="line"><a name="l00283"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#a2dc36d8d2222ed15c7d1eb731ddb7a80">  283</a></span>&#160;  <span class="keyword">virtual</span> <span class="keywordtype">void</span> <a class="code" href="classMPL_1_1env__base.html#a2dc36d8d2222ed15c7d1eb731ddb7a80">set_gradient_weight</a>(<a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> w) {}</div><div class="line"><a name="l00284"></a><span class="lineno">  284</span>&#160;</div><div class="line"><a name="l00286"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#a53f9189d2e1e5dfb9fbd0163e8fd1ee6">  286</a></span>&#160;  <span class="keyword">virtual</span> <span class="keywordtype">void</span> <a class="code" href="classMPL_1_1env__base.html#a53f9189d2e1e5dfb9fbd0163e8fd1ee6">set_potential_map</a>(<span class="keyword">const</span> std::vector&lt;int8_t&gt;&amp; map) {}</div><div class="line"><a name="l00287"></a><span class="lineno">  287</span>&#160;</div><div class="line"><a name="l00289"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#a080b70c332941706c9bd4c37a5ee3d09">  289</a></span>&#160;  <span class="keyword">virtual</span> <span class="keywordtype">void</span> <a class="code" href="classMPL_1_1env__base.html#a080b70c332941706c9bd4c37a5ee3d09">set_gradient_map</a>(<span class="keyword">const</span> <a class="code" href="data__type_8h.html#a6b9210d48a0a541940af71827aad1b41">vec_E</a>&lt;<a class="code" href="data__type_8h.html#a3a0c45655a5e009e56634ccde0c5c575">Vecf&lt;Dim&gt;</a>&gt;&amp; map) {}</div><div class="line"><a name="l00290"></a><span class="lineno">  290</span>&#160;</div><div class="line"><a name="l00292"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#a65f49c4f4b83b84979976a5ebb3f192b">  292</a></span>&#160;  <span class="keywordtype">void</span> <a class="code" href="classMPL_1_1env__base.html#a65f49c4f4b83b84979976a5ebb3f192b">set_t_max</a>(<span class="keywordtype">int</span> t) { <a class="code" href="classMPL_1_1env__base.html#ad592f0117a0ef0ac15225855ae62c4cc">t_max_</a> = t; }</div><div class="line"><a name="l00293"></a><span class="lineno">  293</span>&#160;</div><div class="line"><a name="l00295"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#a82c7c82438a5133e63fa208813a76e28">  295</a></span>&#160;  <span class="keywordtype">bool</span> <a class="code" href="classMPL_1_1env__base.html#a82c7c82438a5133e63fa208813a76e28">set_goal</a>(<span class="keyword">const</span> <a class="code" href="structWaypoint.html">Waypoint&lt;Dim&gt;</a>&amp; state) {</div><div class="line"><a name="l00296"></a><span class="lineno">  296</span>&#160;    <span class="keywordflow">if</span> (<a class="code" href="classMPL_1_1env__base.html#ad12eb1d6268bebc2542a875a3278551a">prior_traj_</a>.empty()) <a class="code" href="classMPL_1_1env__base.html#a8e6706c18fa58da08174f7c0c8209312">goal_node_</a> = state;</div><div class="line"><a name="l00297"></a><span class="lineno">  297</span>&#160;    <span class="keywordflow">return</span> <a class="code" href="classMPL_1_1env__base.html#ad12eb1d6268bebc2542a875a3278551a">prior_traj_</a>.empty();</div><div class="line"><a name="l00298"></a><span class="lineno">  298</span>&#160;  }</div><div class="line"><a name="l00299"></a><span class="lineno">  299</span>&#160;</div><div class="line"><a name="l00301"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#ac736731d272b0b66b0a160e1c2e9a39c">  301</a></span>&#160;  <span class="keywordtype">void</span> <a class="code" href="classMPL_1_1env__base.html#ac736731d272b0b66b0a160e1c2e9a39c">set_search_region</a>(<span class="keyword">const</span> std::vector&lt;bool&gt;&amp; search_region) {</div><div class="line"><a name="l00302"></a><span class="lineno">  302</span>&#160;    <a class="code" href="classMPL_1_1env__base.html#a8873f64d0e619a9945fc3be17b88c227">search_region_</a> = search_region;</div><div class="line"><a name="l00303"></a><span class="lineno">  303</span>&#160;  }</div><div class="line"><a name="l00304"></a><span class="lineno">  304</span>&#160;</div><div class="line"><a name="l00306"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#a2b401fb26c98e874b926795e0d15ab2b">  306</a></span>&#160;  <span class="keywordtype">void</span> <a class="code" href="classMPL_1_1env__base.html#a2b401fb26c98e874b926795e0d15ab2b">set_heur_ignore_dynamics</a>(<span class="keywordtype">bool</span> ignore) { <a class="code" href="classMPL_1_1env__base.html#a12ccadddac2cf7cf643ce4af8982054c">heur_ignore_dynamics_</a> = ignore; }</div><div class="line"><a name="l00307"></a><span class="lineno">  307</span>&#160;</div><div class="line"><a name="l00309"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#a71279da308c3215d1fb8b98cda425fc3">  309</a></span>&#160;  <span class="keyword">virtual</span> <span class="keywordtype">void</span> <a class="code" href="classMPL_1_1env__base.html#a71279da308c3215d1fb8b98cda425fc3">info</a>() {</div><div class="line"><a name="l00310"></a><span class="lineno">  310</span>&#160;    printf(<a class="code" href="data__type_8h.html#a5a123b382640b3aa65dd5db386002fbc">ANSI_COLOR_YELLOW</a> <span class="stringliteral">&quot;\n&quot;</span>);</div><div class="line"><a name="l00311"></a><span class="lineno">  311</span>&#160;    printf(<span class="stringliteral">&quot;++++++++++++++++++++ env_base ++++++++++++++++++\n&quot;</span>);</div><div class="line"><a name="l00312"></a><span class="lineno">  312</span>&#160;    printf(<span class="stringliteral">&quot;+                  w: %.2f               +\n&quot;</span>, <a class="code" href="classMPL_1_1env__base.html#a5c6a2e729131e36e72b51266d887ae8f">w_</a>);</div><div class="line"><a name="l00313"></a><span class="lineno">  313</span>&#160;    printf(<span class="stringliteral">&quot;+               wyaw: %.2f               +\n&quot;</span>, <a class="code" href="classMPL_1_1env__base.html#ad47343816c81f60e207cd424a957c491">wyaw_</a>);</div><div class="line"><a name="l00314"></a><span class="lineno">  314</span>&#160;    printf(<span class="stringliteral">&quot;+                 dt: %.2f               +\n&quot;</span>, <a class="code" href="classMPL_1_1env__base.html#a75662e4867f05ecc21cd1232d2126a11">dt_</a>);</div><div class="line"><a name="l00315"></a><span class="lineno">  315</span>&#160;    printf(<span class="stringliteral">&quot;+              t_max: %.2f               +\n&quot;</span>, <a class="code" href="classMPL_1_1env__base.html#ad592f0117a0ef0ac15225855ae62c4cc">t_max_</a>);</div><div class="line"><a name="l00316"></a><span class="lineno">  316</span>&#160;    printf(<span class="stringliteral">&quot;+              v_max: %.2f               +\n&quot;</span>, <a class="code" href="classMPL_1_1env__base.html#adcd1fa1f64b3b836228d9a79f91dff0e">v_max_</a>);</div><div class="line"><a name="l00317"></a><span class="lineno">  317</span>&#160;    printf(<span class="stringliteral">&quot;+              a_max: %.2f               +\n&quot;</span>, <a class="code" href="classMPL_1_1env__base.html#a7be6859be90f54ef175482d0aa6020c9">a_max_</a>);</div><div class="line"><a name="l00318"></a><span class="lineno">  318</span>&#160;    printf(<span class="stringliteral">&quot;+              j_max: %.2f               +\n&quot;</span>, <a class="code" href="classMPL_1_1env__base.html#a6a67a6409ed46cd0c04f095a18996178">j_max_</a>);</div><div class="line"><a name="l00319"></a><span class="lineno">  319</span>&#160;    printf(<span class="stringliteral">&quot;+            yaw_max: %.2f               +\n&quot;</span>, <a class="code" href="classMPL_1_1env__base.html#a6d1787397e20de82495fb9c381c14980">yaw_max_</a>);</div><div class="line"><a name="l00320"></a><span class="lineno">  320</span>&#160;    printf(<span class="stringliteral">&quot;+              U num: %zu                +\n&quot;</span>, <a class="code" href="classMPL_1_1env__base.html#a632147ec3e706f2bf10fb196c9d0a38c">U_</a>.size());</div><div class="line"><a name="l00321"></a><span class="lineno">  321</span>&#160;    printf(<span class="stringliteral">&quot;+            tol_pos: %.2f               +\n&quot;</span>, <a class="code" href="classMPL_1_1env__base.html#ac85f1da0ec8736ba5c379bbdb6634fe2">tol_pos_</a>);</div><div class="line"><a name="l00322"></a><span class="lineno">  322</span>&#160;    printf(<span class="stringliteral">&quot;+            tol_vel: %.2f               +\n&quot;</span>, <a class="code" href="classMPL_1_1env__base.html#a2a6216a2e0a6fcd1bf2942b145daa889">tol_vel_</a>);</div><div class="line"><a name="l00323"></a><span class="lineno">  323</span>&#160;    printf(<span class="stringliteral">&quot;+            tol_acc: %.2f               +\n&quot;</span>, <a class="code" href="classMPL_1_1env__base.html#ac49a422f3e1b826123a12389024b9c50">tol_acc_</a>);</div><div class="line"><a name="l00324"></a><span class="lineno">  324</span>&#160;    printf(<span class="stringliteral">&quot;+            tol_yaw: %.2f               +\n&quot;</span>, <a class="code" href="classMPL_1_1env__base.html#aee3b399510cbd3dc9690d65f0c099673">tol_yaw_</a>);</div><div class="line"><a name="l00325"></a><span class="lineno">  325</span>&#160;    printf(<span class="stringliteral">&quot;+heur_ignore_dynamics: %d                 +\n&quot;</span>,</div><div class="line"><a name="l00326"></a><span class="lineno">  326</span>&#160;           <a class="code" href="classMPL_1_1env__base.html#a12ccadddac2cf7cf643ce4af8982054c">heur_ignore_dynamics_</a>);</div><div class="line"><a name="l00327"></a><span class="lineno">  327</span>&#160;    printf(<span class="stringliteral">&quot;++++++++++++++++++++ env_base ++++++++++++++++++\n&quot;</span>);</div><div class="line"><a name="l00328"></a><span class="lineno">  328</span>&#160;    printf(<a class="code" href="data__type_8h.html#a92a364c2b863dde1a024a77eac2a5b3b">ANSI_COLOR_RESET</a> <span class="stringliteral">&quot;\n&quot;</span>);</div><div class="line"><a name="l00329"></a><span class="lineno">  329</span>&#160;  }</div><div class="line"><a name="l00330"></a><span class="lineno">  330</span>&#160;</div><div class="line"><a name="l00332"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#a37b1e3307c9779dcc06e11b4a9f694a5">  332</a></span>&#160;  <span class="keyword">virtual</span> <span class="keywordtype">bool</span> <a class="code" href="classMPL_1_1env__base.html#a37b1e3307c9779dcc06e11b4a9f694a5">is_free</a>(<span class="keyword">const</span> <a class="code" href="data__type_8h.html#a3a0c45655a5e009e56634ccde0c5c575">Vecf&lt;Dim&gt;</a>&amp; pt)<span class="keyword"> const </span>{</div><div class="line"><a name="l00333"></a><span class="lineno">  333</span>&#160;    printf(<span class="stringliteral">&quot;Used Null is_free() for pt\n&quot;</span>);</div><div class="line"><a name="l00334"></a><span class="lineno">  334</span>&#160;    <span class="keywordflow">return</span> <span class="keyword">true</span>;</div><div class="line"><a name="l00335"></a><span class="lineno">  335</span>&#160;  }</div><div class="line"><a name="l00336"></a><span class="lineno">  336</span>&#160;</div><div class="line"><a name="l00338"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#a8530f5d66d3ec8b677e67846b83af6d2">  338</a></span>&#160;  <span class="keyword">virtual</span> <span class="keywordtype">bool</span> <a class="code" href="classMPL_1_1env__base.html#a8530f5d66d3ec8b677e67846b83af6d2">is_free</a>(<span class="keyword">const</span> <a class="code" href="classPrimitive.html">Primitive&lt;Dim&gt;</a>&amp; pr)<span class="keyword"> const </span>{</div><div class="line"><a name="l00339"></a><span class="lineno">  339</span>&#160;    printf(<span class="stringliteral">&quot;Used Null is_free() for pr\n&quot;</span>);</div><div class="line"><a name="l00340"></a><span class="lineno">  340</span>&#160;    <span class="keywordflow">return</span> <span class="keyword">true</span>;</div><div class="line"><a name="l00341"></a><span class="lineno">  341</span>&#160;  }</div><div class="line"><a name="l00342"></a><span class="lineno">  342</span>&#160;</div><div class="line"><a name="l00343"></a><span class="lineno">  343</span>&#160;  <span class="keyword">virtual</span> <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> calculate_intrinsic_cost(<span class="keyword">const</span> <a class="code" href="classPrimitive.html">Primitive&lt;Dim&gt;</a>&amp; pr)<span class="keyword"> const </span>{</div><div class="line"><a name="l00344"></a><span class="lineno">  344</span>&#160;    <span class="keywordflow">return</span> pr.<a class="code" href="classPrimitive.html#abebf08092f0450f9dc2796f4b231cf2a">J</a>(pr.<a class="code" href="classPrimitive.html#aa9c00e6dfba12ffc83523e010881ff8f">control</a>()) + <a class="code" href="classMPL_1_1env__base.html#a5c6a2e729131e36e72b51266d887ae8f">w_</a> * <a class="code" href="classMPL_1_1env__base.html#a75662e4867f05ecc21cd1232d2126a11">dt_</a>;</div><div class="line"><a name="l00345"></a><span class="lineno">  345</span>&#160;  }</div><div class="line"><a name="l00346"></a><span class="lineno">  346</span>&#160;</div><div class="line"><a name="l00348"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#a1f189e95f1445250ed2af48d0be09014">  348</a></span>&#160;  <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> <a class="code" href="classMPL_1_1env__base.html#a1f189e95f1445250ed2af48d0be09014">get_dt</a>()<span class="keyword"> const </span>{ <span class="keywordflow">return</span> <a class="code" href="classMPL_1_1env__base.html#a75662e4867f05ecc21cd1232d2126a11">dt_</a>; }</div><div class="line"><a name="l00349"></a><span class="lineno">  349</span>&#160;</div><div class="line"><a name="l00358"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#a79b4bcc2a220c691b742593ca7fd4217">  358</a></span>&#160;  <span class="keyword">virtual</span> <span class="keywordtype">void</span> <a class="code" href="classMPL_1_1env__base.html#a79b4bcc2a220c691b742593ca7fd4217">get_succ</a>(<span class="keyword">const</span> <a class="code" href="structWaypoint.html">Waypoint&lt;Dim&gt;</a>&amp; curr, <a class="code" href="data__type_8h.html#a6b9210d48a0a541940af71827aad1b41">vec_E</a>&lt;<a class="code" href="structWaypoint.html">Waypoint&lt;Dim&gt;</a>&gt;&amp; succ,</div><div class="line"><a name="l00359"></a><span class="lineno">  359</span>&#160;                        std::vector&lt;decimal_t&gt;&amp; succ_cost,</div><div class="line"><a name="l00360"></a><span class="lineno">  360</span>&#160;                        std::vector&lt;int&gt;&amp; action_idx)<span class="keyword"> const </span>{</div><div class="line"><a name="l00361"></a><span class="lineno">  361</span>&#160;    printf(<span class="stringliteral">&quot;Used Null get_succ()\n&quot;</span>);</div><div class="line"><a name="l00362"></a><span class="lineno">  362</span>&#160;  }</div><div class="line"><a name="l00363"></a><span class="lineno">  363</span>&#160;</div><div class="line"><a name="l00365"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#a2ce7924f21de1136dec56c9c2027ba71">  365</a></span>&#160;  std::vector&lt;bool&gt; <a class="code" href="classMPL_1_1env__base.html#a2ce7924f21de1136dec56c9c2027ba71">get_search_region</a>()<span class="keyword"> const </span>{ <span class="keywordflow">return</span> <a class="code" href="classMPL_1_1env__base.html#a8873f64d0e619a9945fc3be17b88c227">search_region_</a>; }</div><div class="line"><a name="l00366"></a><span class="lineno">  366</span>&#160;</div><div class="line"><a name="l00368"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#a12ccadddac2cf7cf643ce4af8982054c">  368</a></span>&#160;  <span class="keywordtype">bool</span> <a class="code" href="classMPL_1_1env__base.html#a12ccadddac2cf7cf643ce4af8982054c">heur_ignore_dynamics_</a>{<span class="keyword">true</span>};</div><div class="line"><a name="l00370"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#a5c6a2e729131e36e72b51266d887ae8f">  370</a></span>&#160;  <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> <a class="code" href="classMPL_1_1env__base.html#a5c6a2e729131e36e72b51266d887ae8f">w_</a>{10.0};</div><div class="line"><a name="l00372"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#ad47343816c81f60e207cd424a957c491">  372</a></span>&#160;  <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> <a class="code" href="classMPL_1_1env__base.html#ad47343816c81f60e207cd424a957c491">wyaw_</a>{1.0};</div><div class="line"><a name="l00374"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#ac85f1da0ec8736ba5c379bbdb6634fe2">  374</a></span>&#160;  <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> <a class="code" href="classMPL_1_1env__base.html#ac85f1da0ec8736ba5c379bbdb6634fe2">tol_pos_</a>{0.5};</div><div class="line"><a name="l00376"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#a2a6216a2e0a6fcd1bf2942b145daa889">  376</a></span>&#160;  <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> <a class="code" href="classMPL_1_1env__base.html#a2a6216a2e0a6fcd1bf2942b145daa889">tol_vel_</a>{-1.0};</div><div class="line"><a name="l00378"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#ac49a422f3e1b826123a12389024b9c50">  378</a></span>&#160;  <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> <a class="code" href="classMPL_1_1env__base.html#ac49a422f3e1b826123a12389024b9c50">tol_acc_</a>{-1.0};</div><div class="line"><a name="l00380"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#aee3b399510cbd3dc9690d65f0c099673">  380</a></span>&#160;  <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> <a class="code" href="classMPL_1_1env__base.html#aee3b399510cbd3dc9690d65f0c099673">tol_yaw_</a>{-1.0};</div><div class="line"><a name="l00382"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#adcd1fa1f64b3b836228d9a79f91dff0e">  382</a></span>&#160;  <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> <a class="code" href="classMPL_1_1env__base.html#adcd1fa1f64b3b836228d9a79f91dff0e">v_max_</a>{-1.0};</div><div class="line"><a name="l00384"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#a7be6859be90f54ef175482d0aa6020c9">  384</a></span>&#160;  <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> <a class="code" href="classMPL_1_1env__base.html#a7be6859be90f54ef175482d0aa6020c9">a_max_</a>{-1.0};</div><div class="line"><a name="l00386"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#a6a67a6409ed46cd0c04f095a18996178">  386</a></span>&#160;  <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> <a class="code" href="classMPL_1_1env__base.html#a6a67a6409ed46cd0c04f095a18996178">j_max_</a>{-1.0};</div><div class="line"><a name="l00388"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#a6d1787397e20de82495fb9c381c14980">  388</a></span>&#160;  <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> <a class="code" href="classMPL_1_1env__base.html#a6d1787397e20de82495fb9c381c14980">yaw_max_</a>{-1.0};</div><div class="line"><a name="l00390"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#ad592f0117a0ef0ac15225855ae62c4cc">  390</a></span>&#160;  <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> <a class="code" href="classMPL_1_1env__base.html#ad592f0117a0ef0ac15225855ae62c4cc">t_max_</a>{std::numeric_limits&lt;decimal_t&gt;::infinity()};</div><div class="line"><a name="l00392"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#a75662e4867f05ecc21cd1232d2126a11">  392</a></span>&#160;  <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> <a class="code" href="classMPL_1_1env__base.html#a75662e4867f05ecc21cd1232d2126a11">dt_</a>{1.0};</div><div class="line"><a name="l00394"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#a632147ec3e706f2bf10fb196c9d0a38c">  394</a></span>&#160;  <a class="code" href="data__type_8h.html#a6b9210d48a0a541940af71827aad1b41">vec_E&lt;VecDf&gt;</a> <a class="code" href="classMPL_1_1env__base.html#a632147ec3e706f2bf10fb196c9d0a38c">U_</a>;</div><div class="line"><a name="l00396"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#a8e6706c18fa58da08174f7c0c8209312">  396</a></span>&#160;  <a class="code" href="structWaypoint.html">Waypoint&lt;Dim&gt;</a> <a class="code" href="classMPL_1_1env__base.html#a8e6706c18fa58da08174f7c0c8209312">goal_node_</a>;</div><div class="line"><a name="l00398"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#ad12eb1d6268bebc2542a875a3278551a">  398</a></span>&#160;  <a class="code" href="data__type_8h.html#a6b9210d48a0a541940af71827aad1b41">vec_E&lt;std::pair&lt;Waypoint&lt;Dim&gt;</a>, <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a>&gt;&gt; <a class="code" href="classMPL_1_1env__base.html#ad12eb1d6268bebc2542a875a3278551a">prior_traj_</a>;</div><div class="line"><a name="l00400"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#a8873f64d0e619a9945fc3be17b88c227">  400</a></span>&#160;  std::vector&lt;bool&gt; <a class="code" href="classMPL_1_1env__base.html#a8873f64d0e619a9945fc3be17b88c227">search_region_</a>;</div><div class="line"><a name="l00402"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#a407c61fa8bfdefc3012546f0c412acd3">  402</a></span>&#160;  <span class="keyword">mutable</span> <a class="code" href="data__type_8h.html#a0a3b37beb40c06edc02cb4b7339f2b96">vec_Vecf&lt;Dim&gt;</a> <a class="code" href="classMPL_1_1env__base.html#a407c61fa8bfdefc3012546f0c412acd3">expanded_nodes_</a>;</div><div class="line"><a name="l00404"></a><span class="lineno"><a class="line" href="classMPL_1_1env__base.html#a1d96af4d1aa7e1a15924316fae89174e">  404</a></span>&#160;  <span class="keyword">mutable</span> <a class="code" href="data__type_8h.html#a6b9210d48a0a541940af71827aad1b41">vec_E&lt;Primitive&lt;Dim&gt;</a>&gt; <a class="code" href="classMPL_1_1env__base.html#a1d96af4d1aa7e1a15924316fae89174e">expanded_edges_</a>;</div><div class="line"><a name="l00405"></a><span class="lineno">  405</span>&#160;};</div><div class="line"><a name="l00406"></a><span class="lineno">  406</span>&#160;}  <span class="comment">// namespace MPL</span></div><div class="line"><a name="l00407"></a><span class="lineno">  407</span>&#160;</div><div class="line"><a name="l00408"></a><span class="lineno">  408</span>&#160;<span class="preprocessor">#endif</span></div><div class="ttc" id="classMPL_1_1env__base_html_a79b4bcc2a220c691b742593ca7fd4217"><div class="ttname"><a href="classMPL_1_1env__base.html#a79b4bcc2a220c691b742593ca7fd4217">MPL::env_base::get_succ</a></div><div class="ttdeci">virtual void get_succ(const Waypoint&lt; Dim &gt; &amp;curr, vec_E&lt; Waypoint&lt; Dim &gt;&gt; &amp;succ, std::vector&lt; decimal_t &gt; &amp;succ_cost, std::vector&lt; int &gt; &amp;action_idx) const</div><div class="ttdoc">Get successor. </div><div class="ttdef"><b>Definition:</b> env_base.h:358</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_a6bda580c1bc7ca88ce2671f1533fc08a"><div class="ttname"><a href="classMPL_1_1env__base.html#a6bda580c1bc7ca88ce2671f1533fc08a">MPL::env_base::cal_heur</a></div><div class="ttdeci">virtual decimal_t cal_heur(const Waypoint&lt; Dim &gt; &amp;state, const Waypoint&lt; Dim &gt; &amp;goal) const</div><div class="ttdoc">calculate the cost from state to goal </div><div class="ttdef"><b>Definition:</b> env_base.h:56</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_ad12eb1d6268bebc2542a875a3278551a"><div class="ttname"><a href="classMPL_1_1env__base.html#ad12eb1d6268bebc2542a875a3278551a">MPL::env_base::prior_traj_</a></div><div class="ttdeci">vec_E&lt; std::pair&lt; Waypoint&lt; Dim &gt;, decimal_t &gt; &gt; prior_traj_</div><div class="ttdoc">Prior trajectory. </div><div class="ttdef"><b>Definition:</b> env_base.h:398</div></div>
<div class="ttc" id="classTrajectory_html_aec62c9d9b1a35202df2072ea3efcef09"><div class="ttname"><a href="classTrajectory.html#aec62c9d9b1a35202df2072ea3efcef09">Trajectory::evaluate</a></div><div class="ttdeci">Waypoint&lt; Dim &gt; evaluate(decimal_t time) const</div><div class="ttdoc">Evaluate Waypoint at time . </div><div class="ttdef"><b>Definition:</b> trajectory.h:67</div></div>
<div class="ttc" id="math_8h_html_a11e53038927f416e637df83d4a0b18bc"><div class="ttname"><a href="math_8h.html#a11e53038927f416e637df83d4a0b18bc">quartic</a></div><div class="ttdeci">std::vector&lt; decimal_t &gt; quartic(decimal_t a, decimal_t b, decimal_t c, decimal_t d, decimal_t e)</div><div class="ttdoc">Quartic equation: . </div><div class="ttdef"><b>Definition:</b> math.h:69</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_a8873f64d0e619a9945fc3be17b88c227"><div class="ttname"><a href="classMPL_1_1env__base.html#a8873f64d0e619a9945fc3be17b88c227">MPL::env_base::search_region_</a></div><div class="ttdeci">std::vector&lt; bool &gt; search_region_</div><div class="ttdoc">Valid search region (tunnel constraint) </div><div class="ttdef"><b>Definition:</b> env_base.h:400</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_a2a6216a2e0a6fcd1bf2942b145daa889"><div class="ttname"><a href="classMPL_1_1env__base.html#a2a6216a2e0a6fcd1bf2942b145daa889">MPL::env_base::tol_vel_</a></div><div class="ttdeci">decimal_t tol_vel_</div><div class="ttdoc">tolerance of velocity for goal region, 0 means no tolerance </div><div class="ttdef"><b>Definition:</b> env_base.h:376</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_a1d96af4d1aa7e1a15924316fae89174e"><div class="ttname"><a href="classMPL_1_1env__base.html#a1d96af4d1aa7e1a15924316fae89174e">MPL::env_base::expanded_edges_</a></div><div class="ttdeci">vec_E&lt; Primitive&lt; Dim &gt; &gt; expanded_edges_</div><div class="ttdoc">expanded edges for debug </div><div class="ttdef"><b>Definition:</b> env_base.h:404</div></div>
<div class="ttc" id="namespaceMPL_html"><div class="ttname"><a href="namespaceMPL.html">MPL</a></div><div class="ttdef"><b>Definition:</b> map_util.h:12</div></div>
<div class="ttc" id="trajectory_8h_html"><div class="ttname"><a href="trajectory_8h.html">trajectory.h</a></div><div class="ttdoc">Trajectory class. </div></div>
<div class="ttc" id="classMPL_1_1env__base_html_a22bf748adf6a4febbc7b4b943e76940a"><div class="ttname"><a href="classMPL_1_1env__base.html#a22bf748adf6a4febbc7b4b943e76940a">MPL::env_base::forward_action</a></div><div class="ttdeci">void forward_action(const Waypoint&lt; Dim &gt; &amp;curr, int action_id, Primitive&lt; Dim &gt; &amp;pr) const</div><div class="ttdoc">Recover trajectory. </div><div class="ttdef"><b>Definition:</b> env_base.h:228</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_ad47343816c81f60e207cd424a957c491"><div class="ttname"><a href="classMPL_1_1env__base.html#ad47343816c81f60e207cd424a957c491">MPL::env_base::wyaw_</a></div><div class="ttdeci">decimal_t wyaw_</div><div class="ttdoc">weight of yaw </div><div class="ttdef"><b>Definition:</b> env_base.h:372</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_a080b70c332941706c9bd4c37a5ee3d09"><div class="ttname"><a href="classMPL_1_1env__base.html#a080b70c332941706c9bd4c37a5ee3d09">MPL::env_base::set_gradient_map</a></div><div class="ttdeci">virtual void set_gradient_map(const vec_E&lt; Vecf&lt; Dim &gt;&gt; &amp;map)</div><div class="ttdoc">set weight for cost in time, usually no need to change </div><div class="ttdef"><b>Definition:</b> env_base.h:289</div></div>
<div class="ttc" id="classPrimitive_html_abebf08092f0450f9dc2796f4b231cf2a"><div class="ttname"><a href="classPrimitive.html#abebf08092f0450f9dc2796f4b231cf2a">Primitive::J</a></div><div class="ttdeci">decimal_t J(const Control::Control &amp;control) const</div><div class="ttdoc">Return total efforts for the given duration. </div><div class="ttdef"><b>Definition:</b> primitive.h:403</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_ae01a34b11afd9c21168d04a009f12248"><div class="ttname"><a href="classMPL_1_1env__base.html#ae01a34b11afd9c21168d04a009f12248">MPL::env_base::set_potential_weight</a></div><div class="ttdeci">virtual void set_potential_weight(decimal_t w)</div><div class="ttdoc">set weight for cost in time, usually no need to change </div><div class="ttdef"><b>Definition:</b> env_base.h:280</div></div>
<div class="ttc" id="structWaypoint_html_ad5c2f2b8f6dba96bb93faccde844e9a8"><div class="ttname"><a href="structWaypoint.html#ad5c2f2b8f6dba96bb93faccde844e9a8">Waypoint::yaw</a></div><div class="ttdeci">decimal_t yaw</div><div class="ttdoc">yaw </div><div class="ttdef"><b>Definition:</b> waypoint.h:36</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_a868d8d096eff02ddc71dc7163271e671"><div class="ttname"><a href="classMPL_1_1env__base.html#a868d8d096eff02ddc71dc7163271e671">MPL::env_base::set_tol_vel</a></div><div class="ttdeci">void set_tol_vel(decimal_t vel)</div><div class="ttdoc">Set velocity tolerance for goal region. </div><div class="ttdef"><b>Definition:</b> env_base.h:265</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_aaff1cc4715bc47e32ef95d82a68d47d1"><div class="ttname"><a href="classMPL_1_1env__base.html#aaff1cc4715bc47e32ef95d82a68d47d1">MPL::env_base::set_yaw_max</a></div><div class="ttdeci">void set_yaw_max(decimal_t yaw)</div><div class="ttdoc">Set max acc in each axis. </div><div class="ttdef"><b>Definition:</b> env_base.h:246</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_ac736731d272b0b66b0a160e1c2e9a39c"><div class="ttname"><a href="classMPL_1_1env__base.html#ac736731d272b0b66b0a160e1c2e9a39c">MPL::env_base::set_search_region</a></div><div class="ttdeci">void set_search_region(const std::vector&lt; bool &gt; &amp;search_region)</div><div class="ttdoc">Set valid search region (tunnel constraint) </div><div class="ttdef"><b>Definition:</b> env_base.h:301</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_a3f5a3aa9e6be63c4bbfe96e09b56a68b"><div class="ttname"><a href="classMPL_1_1env__base.html#a3f5a3aa9e6be63c4bbfe96e09b56a68b">MPL::env_base::get_heur</a></div><div class="ttdeci">virtual decimal_t get_heur(const Waypoint&lt; Dim &gt; &amp;state) const</div><div class="ttdoc">Heuristic function. </div><div class="ttdef"><b>Definition:</b> env_base.h:46</div></div>
<div class="ttc" id="structWaypoint_html"><div class="ttname"><a href="structWaypoint.html">Waypoint</a></div><div class="ttdoc">Waypoint base class. </div><div class="ttdef"><b>Definition:</b> waypoint.h:23</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_adfea242d77a013b39d94b8e5da4289fe"><div class="ttname"><a href="classMPL_1_1env__base.html#adfea242d77a013b39d94b8e5da4289fe">MPL::env_base::is_goal</a></div><div class="ttdeci">virtual bool is_goal(const Waypoint&lt; Dim &gt; &amp;state) const</div><div class="ttdoc">Check if state hit the goal region, use L-1 norm. </div><div class="ttdef"><b>Definition:</b> env_base.h:23</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_a82c7c82438a5133e63fa208813a76e28"><div class="ttname"><a href="classMPL_1_1env__base.html#a82c7c82438a5133e63fa208813a76e28">MPL::env_base::set_goal</a></div><div class="ttdeci">bool set_goal(const Waypoint&lt; Dim &gt; &amp;state)</div><div class="ttdoc">Set goal state. </div><div class="ttdef"><b>Definition:</b> env_base.h:295</div></div>
<div class="ttc" id="classTrajectory_html_a93f4d5f86feb5f56e5fae7fd02311e9c"><div class="ttname"><a href="classTrajectory.html#a93f4d5f86feb5f56e5fae7fd02311e9c">Trajectory::getTotalTime</a></div><div class="ttdeci">decimal_t getTotalTime() const</div><div class="ttdoc">Get the total duration of Trajectory. </div><div class="ttdef"><b>Definition:</b> trajectory.h:300</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_a632147ec3e706f2bf10fb196c9d0a38c"><div class="ttname"><a href="classMPL_1_1env__base.html#a632147ec3e706f2bf10fb196c9d0a38c">MPL::env_base::U_</a></div><div class="ttdeci">vec_E&lt; VecDf &gt; U_</div><div class="ttdoc">Array of constant control input. </div><div class="ttdef"><b>Definition:</b> env_base.h:394</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_ac49a422f3e1b826123a12389024b9c50"><div class="ttname"><a href="classMPL_1_1env__base.html#ac49a422f3e1b826123a12389024b9c50">MPL::env_base::tol_acc_</a></div><div class="ttdeci">decimal_t tol_acc_</div><div class="ttdoc">tolerance of acceleration for goal region, 0 means no tolerance </div><div class="ttdef"><b>Definition:</b> env_base.h:378</div></div>
<div class="ttc" id="classTrajectory_html"><div class="ttname"><a href="classTrajectory.html">Trajectory</a></div><div class="ttdoc">Trajectory class. </div><div class="ttdef"><b>Definition:</b> trajectory.h:43</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_a84764f976374a3964c5299ce837fa762"><div class="ttname"><a href="classMPL_1_1env__base.html#a84764f976374a3964c5299ce837fa762">MPL::env_base::set_prior_trajectory</a></div><div class="ttdeci">virtual void set_prior_trajectory(const Trajectory&lt; Dim &gt; &amp;traj)</div><div class="ttdoc">Set prior trajectory. </div><div class="ttdef"><b>Definition:</b> env_base.h:249</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_aee3b399510cbd3dc9690d65f0c099673"><div class="ttname"><a href="classMPL_1_1env__base.html#aee3b399510cbd3dc9690d65f0c099673">MPL::env_base::tol_yaw_</a></div><div class="ttdeci">decimal_t tol_yaw_</div><div class="ttdoc">tolerance of yaw for goal region, 0 means no tolerance </div><div class="ttdef"><b>Definition:</b> env_base.h:380</div></div>
<div class="ttc" id="math_8h_html_a981120793ddb8cdf9ce466e8c2e70e4b"><div class="ttname"><a href="math_8h.html#a981120793ddb8cdf9ce466e8c2e70e4b">solve</a></div><div class="ttdeci">std::vector&lt; decimal_t &gt; solve(decimal_t a, decimal_t b, decimal_t c, decimal_t d, decimal_t e)</div><div class="ttdoc">General solver for . </div><div class="ttdef"><b>Definition:</b> math.h:117</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_aa929ae0eb7125962b07a3fcff970251d"><div class="ttname"><a href="classMPL_1_1env__base.html#aa929ae0eb7125962b07a3fcff970251d">MPL::env_base::to_string</a></div><div class="ttdeci">std::string to_string(const Veci&lt; Dim &gt; &amp;vec) const</div><div class="ttdoc">Convert a vec to a string. </div><div class="ttdef"><b>Definition:</b> env_base.h:221</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_a78ffc9bb089726cf9813d57cfef43d03"><div class="ttname"><a href="classMPL_1_1env__base.html#a78ffc9bb089726cf9813d57cfef43d03">MPL::env_base::set_w</a></div><div class="ttdeci">void set_w(decimal_t w)</div><div class="ttdoc">set weight for cost in time, usually no need to change </div><div class="ttdef"><b>Definition:</b> env_base.h:274</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_a6d1787397e20de82495fb9c381c14980"><div class="ttname"><a href="classMPL_1_1env__base.html#a6d1787397e20de82495fb9c381c14980">MPL::env_base::yaw_max_</a></div><div class="ttdeci">decimal_t yaw_max_</div><div class="ttdoc">max yaw </div><div class="ttdef"><b>Definition:</b> env_base.h:388</div></div>
<div class="ttc" id="structWaypoint_html_a648217ed387d4e226bec8ecb05bfefe8"><div class="ttname"><a href="structWaypoint.html#a648217ed387d4e226bec8ecb05bfefe8">Waypoint::acc</a></div><div class="ttdeci">Vecf&lt; Dim &gt; acc</div><div class="ttdoc">acceleration in  </div><div class="ttdef"><b>Definition:</b> waypoint.h:34</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_a1f189e95f1445250ed2af48d0be09014"><div class="ttname"><a href="classMPL_1_1env__base.html#a1f189e95f1445250ed2af48d0be09014">MPL::env_base::get_dt</a></div><div class="ttdeci">decimal_t get_dt() const</div><div class="ttdoc">Retrieve dt. </div><div class="ttdef"><b>Definition:</b> env_base.h:348</div></div>
<div class="ttc" id="data__type_8h_html_a6b9210d48a0a541940af71827aad1b41"><div class="ttname"><a href="data__type_8h.html#a6b9210d48a0a541940af71827aad1b41">vec_E</a></div><div class="ttdeci">std::vector&lt; T, Eigen::aligned_allocator&lt; T &gt; &gt; vec_E</div><div class="ttdoc">Pre-allocated std::vector for Eigen using vec_E. </div><div class="ttdef"><b>Definition:</b> data_type.h:53</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_adcd1fa1f64b3b836228d9a79f91dff0e"><div class="ttname"><a href="classMPL_1_1env__base.html#adcd1fa1f64b3b836228d9a79f91dff0e">MPL::env_base::v_max_</a></div><div class="ttdeci">decimal_t v_max_</div><div class="ttdoc">max velocity </div><div class="ttdef"><b>Definition:</b> env_base.h:382</div></div>
<div class="ttc" id="structWaypoint_html_a32d4053f0b72324f20f1d1804fe37755"><div class="ttname"><a href="structWaypoint.html#a32d4053f0b72324f20f1d1804fe37755">Waypoint::control</a></div><div class="ttdeci">Control::Control control</div><div class="ttdoc">Control value. </div><div class="ttdef"><b>Definition:</b> waypoint.h:54</div></div>
<div class="ttc" id="data__type_8h_html_a0a3b37beb40c06edc02cb4b7339f2b96"><div class="ttname"><a href="data__type_8h.html#a0a3b37beb40c06edc02cb4b7339f2b96">vec_Vecf</a></div><div class="ttdeci">vec_E&lt; Vecf&lt; N &gt; &gt; vec_Vecf</div><div class="ttdoc">Vector of Eigen 1D float vector. </div><div class="ttdef"><b>Definition:</b> data_type.h:70</div></div>
<div class="ttc" id="structWaypoint_html_a91393e0d8d73afb9df512710e56f02a5"><div class="ttname"><a href="structWaypoint.html#a91393e0d8d73afb9df512710e56f02a5">Waypoint::vel</a></div><div class="ttdeci">Vecf&lt; Dim &gt; vel</div><div class="ttdoc">velocity in  </div><div class="ttdef"><b>Definition:</b> waypoint.h:33</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_a17295278edfcd6cbc265d01371aac43f"><div class="ttname"><a href="classMPL_1_1env__base.html#a17295278edfcd6cbc265d01371aac43f">MPL::env_base::set_a_max</a></div><div class="ttdeci">void set_a_max(decimal_t a)</div><div class="ttdoc">Set max acc in each axis. </div><div class="ttdef"><b>Definition:</b> env_base.h:240</div></div>
<div class="ttc" id="structWaypoint_html_a9661067e17763fffb65d99cabaff802a"><div class="ttname"><a href="structWaypoint.html#a9661067e17763fffb65d99cabaff802a">Waypoint::t</a></div><div class="ttdeci">decimal_t t</div><div class="ttdoc">time when reaching this waypoint in graph search </div><div class="ttdef"><b>Definition:</b> waypoint.h:37</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_a5fe3e7b63d589bbf1ccec75cb28e6626"><div class="ttname"><a href="classMPL_1_1env__base.html#a5fe3e7b63d589bbf1ccec75cb28e6626">MPL::env_base::set_u</a></div><div class="ttdeci">void set_u(const vec_E&lt; VecDf &gt; &amp;U)</div><div class="ttdoc">Set control input. </div><div class="ttdef"><b>Definition:</b> env_base.h:234</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_ad592f0117a0ef0ac15225855ae62c4cc"><div class="ttname"><a href="classMPL_1_1env__base.html#ad592f0117a0ef0ac15225855ae62c4cc">MPL::env_base::t_max_</a></div><div class="ttdeci">decimal_t t_max_</div><div class="ttdoc">max time </div><div class="ttdef"><b>Definition:</b> env_base.h:390</div></div>
<div class="ttc" id="data__type_8h_html_ac208833a6aa7ec29f39eb5ae142aaede"><div class="ttname"><a href="data__type_8h.html#ac208833a6aa7ec29f39eb5ae142aaede">Veci</a></div><div class="ttdeci">Eigen::Matrix&lt; int, N, 1 &gt; Veci</div><div class="ttdoc">Eigen 1D int vector of size N. </div><div class="ttdef"><b>Definition:</b> data_type.h:59</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_a2ce7924f21de1136dec56c9c2027ba71"><div class="ttname"><a href="classMPL_1_1env__base.html#a2ce7924f21de1136dec56c9c2027ba71">MPL::env_base::get_search_region</a></div><div class="ttdeci">std::vector&lt; bool &gt; get_search_region() const</div><div class="ttdoc">Get the valid region. </div><div class="ttdef"><b>Definition:</b> env_base.h:365</div></div>
<div class="ttc" id="data__type_8h_html_a7c99d9360fc6cac2762b786e2fb52266"><div class="ttname"><a href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a></div><div class="ttdeci">double decimal_t</div><div class="ttdoc">Rename the float type used in lib. </div><div class="ttdef"><b>Definition:</b> data_type.h:49</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_a75662e4867f05ecc21cd1232d2126a11"><div class="ttname"><a href="classMPL_1_1env__base.html#a75662e4867f05ecc21cd1232d2126a11">MPL::env_base::dt_</a></div><div class="ttdeci">decimal_t dt_</div><div class="ttdoc">duration of primitive </div><div class="ttdef"><b>Definition:</b> env_base.h:392</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_a6a67a6409ed46cd0c04f095a18996178"><div class="ttname"><a href="classMPL_1_1env__base.html#a6a67a6409ed46cd0c04f095a18996178">MPL::env_base::j_max_</a></div><div class="ttdeci">decimal_t j_max_</div><div class="ttdoc">max jerk </div><div class="ttdef"><b>Definition:</b> env_base.h:386</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_a407c61fa8bfdefc3012546f0c412acd3"><div class="ttname"><a href="classMPL_1_1env__base.html#a407c61fa8bfdefc3012546f0c412acd3">MPL::env_base::expanded_nodes_</a></div><div class="ttdeci">vec_Vecf&lt; Dim &gt; expanded_nodes_</div><div class="ttdoc">expanded nodes for debug </div><div class="ttdef"><b>Definition:</b> env_base.h:402</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_aa9a38f44386ae7778411212b63fa42b7"><div class="ttname"><a href="classMPL_1_1env__base.html#aa9a38f44386ae7778411212b63fa42b7">MPL::env_base::round</a></div><div class="ttdeci">Veci&lt; Dim &gt; round(const Vecf&lt; Dim &gt; &amp;vec, decimal_t res) const</div><div class="ttdoc">Replace the original cast function. </div><div class="ttdef"><b>Definition:</b> env_base.h:214</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_a8e6706c18fa58da08174f7c0c8209312"><div class="ttname"><a href="classMPL_1_1env__base.html#a8e6706c18fa58da08174f7c0c8209312">MPL::env_base::goal_node_</a></div><div class="ttdeci">Waypoint&lt; Dim &gt; goal_node_</div><div class="ttdoc">Goal node. </div><div class="ttdef"><b>Definition:</b> env_base.h:396</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_a8530f5d66d3ec8b677e67846b83af6d2"><div class="ttname"><a href="classMPL_1_1env__base.html#a8530f5d66d3ec8b677e67846b83af6d2">MPL::env_base::is_free</a></div><div class="ttdeci">virtual bool is_free(const Primitive&lt; Dim &gt; &amp;pr) const</div><div class="ttdoc">Check if a primitive is in free space. </div><div class="ttdef"><b>Definition:</b> env_base.h:338</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_a53f9189d2e1e5dfb9fbd0163e8fd1ee6"><div class="ttname"><a href="classMPL_1_1env__base.html#a53f9189d2e1e5dfb9fbd0163e8fd1ee6">MPL::env_base::set_potential_map</a></div><div class="ttdeci">virtual void set_potential_map(const std::vector&lt; int8_t &gt; &amp;map)</div><div class="ttdoc">set weight for cost in time, usually no need to change </div><div class="ttdef"><b>Definition:</b> env_base.h:286</div></div>
<div class="ttc" id="data__type_8h_html_a92a364c2b863dde1a024a77eac2a5b3b"><div class="ttname"><a href="data__type_8h.html#a92a364c2b863dde1a024a77eac2a5b3b">ANSI_COLOR_RESET</a></div><div class="ttdeci">#define ANSI_COLOR_RESET</div><div class="ttdoc">Reset font color in printf funtion. </div><div class="ttdef"><b>Definition:</b> data_type.h:40</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_a7be6859be90f54ef175482d0aa6020c9"><div class="ttname"><a href="classMPL_1_1env__base.html#a7be6859be90f54ef175482d0aa6020c9">MPL::env_base::a_max_</a></div><div class="ttdeci">decimal_t a_max_</div><div class="ttdoc">max acceleration </div><div class="ttdef"><b>Definition:</b> env_base.h:384</div></div>
<div class="ttc" id="data__type_8h_html_a3a0c45655a5e009e56634ccde0c5c575"><div class="ttname"><a href="data__type_8h.html#a3a0c45655a5e009e56634ccde0c5c575">Vecf</a></div><div class="ttdeci">Eigen::Matrix&lt; decimal_t, N, 1 &gt; Vecf</div><div class="ttdoc">Eigen 1D float vector of size N. </div><div class="ttdef"><b>Definition:</b> data_type.h:56</div></div>
<div class="ttc" id="classMPL_1_1env__base_html"><div class="ttname"><a href="classMPL_1_1env__base.html">MPL::env_base</a></div><div class="ttdoc">Base environment class. </div><div class="ttdef"><b>Definition:</b> env_base.h:17</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_a37b1e3307c9779dcc06e11b4a9f694a5"><div class="ttname"><a href="classMPL_1_1env__base.html#a37b1e3307c9779dcc06e11b4a9f694a5">MPL::env_base::is_free</a></div><div class="ttdeci">virtual bool is_free(const Vecf&lt; Dim &gt; &amp;pt) const</div><div class="ttdoc">Check if a point is in free space. </div><div class="ttdef"><b>Definition:</b> env_base.h:332</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_a5c6a2e729131e36e72b51266d887ae8f"><div class="ttname"><a href="classMPL_1_1env__base.html#a5c6a2e729131e36e72b51266d887ae8f">MPL::env_base::w_</a></div><div class="ttdeci">decimal_t w_</div><div class="ttdoc">weight of time cost </div><div class="ttdef"><b>Definition:</b> env_base.h:370</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_af8408acf8e66abd1d07f69d55e03a4dc"><div class="ttname"><a href="classMPL_1_1env__base.html#af8408acf8e66abd1d07f69d55e03a4dc">MPL::env_base::set_dt</a></div><div class="ttdeci">void set_dt(decimal_t dt)</div><div class="ttdoc">Set dt for primitive. </div><div class="ttdef"><b>Definition:</b> env_base.h:259</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_a12ccadddac2cf7cf643ce4af8982054c"><div class="ttname"><a href="classMPL_1_1env__base.html#a12ccadddac2cf7cf643ce4af8982054c">MPL::env_base::heur_ignore_dynamics_</a></div><div class="ttdeci">bool heur_ignore_dynamics_</div><div class="ttdoc">if enabled, ignore dynamics when calculate heuristic </div><div class="ttdef"><b>Definition:</b> env_base.h:368</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_a9cb1a2bea89036d66138005f2c90c903"><div class="ttname"><a href="classMPL_1_1env__base.html#a9cb1a2bea89036d66138005f2c90c903">MPL::env_base::set_wyaw</a></div><div class="ttdeci">void set_wyaw(decimal_t wyaw)</div><div class="ttdoc">set weight for cost in yaw, usually no need to change </div><div class="ttdef"><b>Definition:</b> env_base.h:277</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_aa2209dde7b5a71de3d4992232203d29a"><div class="ttname"><a href="classMPL_1_1env__base.html#aa2209dde7b5a71de3d4992232203d29a">MPL::env_base::set_tol_yaw</a></div><div class="ttdeci">void set_tol_yaw(decimal_t yaw)</div><div class="ttdoc">Set acceleration tolerance for goal region. </div><div class="ttdef"><b>Definition:</b> env_base.h:271</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_a2dc36d8d2222ed15c7d1eb731ddb7a80"><div class="ttname"><a href="classMPL_1_1env__base.html#a2dc36d8d2222ed15c7d1eb731ddb7a80">MPL::env_base::set_gradient_weight</a></div><div class="ttdeci">virtual void set_gradient_weight(decimal_t w)</div><div class="ttdoc">set weight for cost in time, usually no need to change </div><div class="ttdef"><b>Definition:</b> env_base.h:283</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_a8c3490b413cb9a37dd40c8e250763072"><div class="ttname"><a href="classMPL_1_1env__base.html#a8c3490b413cb9a37dd40c8e250763072">MPL::env_base::set_tol_pos</a></div><div class="ttdeci">void set_tol_pos(decimal_t pos)</div><div class="ttdoc">Set distance tolerance for goal region. </div><div class="ttdef"><b>Definition:</b> env_base.h:262</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_a65f49c4f4b83b84979976a5ebb3f192b"><div class="ttname"><a href="classMPL_1_1env__base.html#a65f49c4f4b83b84979976a5ebb3f192b">MPL::env_base::set_t_max</a></div><div class="ttdeci">void set_t_max(int t)</div><div class="ttdoc">Set max time. </div><div class="ttdef"><b>Definition:</b> env_base.h:292</div></div>
<div class="ttc" id="classPrimitive_html"><div class="ttname"><a href="classPrimitive.html">Primitive</a></div><div class="ttdoc">Primitive class. </div><div class="ttdef"><b>Definition:</b> primitive.h:206</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_a73390f0939fc1efd123696f3e805f98e"><div class="ttname"><a href="classMPL_1_1env__base.html#a73390f0939fc1efd123696f3e805f98e">MPL::env_base::env_base</a></div><div class="ttdeci">env_base()</div><div class="ttdoc">Simple constructor. </div><div class="ttdef"><b>Definition:</b> env_base.h:20</div></div>
<div class="ttc" id="data__type_8h_html_a5a123b382640b3aa65dd5db386002fbc"><div class="ttname"><a href="data__type_8h.html#a5a123b382640b3aa65dd5db386002fbc">ANSI_COLOR_YELLOW</a></div><div class="ttdeci">#define ANSI_COLOR_YELLOW</div><div class="ttdoc">Set yellow font in printf funtion. </div><div class="ttdef"><b>Definition:</b> data_type.h:24</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_a2b401fb26c98e874b926795e0d15ab2b"><div class="ttname"><a href="classMPL_1_1env__base.html#a2b401fb26c98e874b926795e0d15ab2b">MPL::env_base::set_heur_ignore_dynamics</a></div><div class="ttdeci">void set_heur_ignore_dynamics(bool ignore)</div><div class="ttdoc">Set heur_ignore_dynamics. </div><div class="ttdef"><b>Definition:</b> env_base.h:306</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_a71279da308c3215d1fb8b98cda425fc3"><div class="ttname"><a href="classMPL_1_1env__base.html#a71279da308c3215d1fb8b98cda425fc3">MPL::env_base::info</a></div><div class="ttdeci">virtual void info()</div><div class="ttdoc">Print out params. </div><div class="ttdef"><b>Definition:</b> env_base.h:309</div></div>
<div class="ttc" id="structWaypoint_html_af63de420a521f105eea4ce098a51ad7e"><div class="ttname"><a href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">Waypoint::pos</a></div><div class="ttdeci">Vecf&lt; Dim &gt; pos</div><div class="ttdoc">position in  </div><div class="ttdef"><b>Definition:</b> waypoint.h:32</div></div>
<div class="ttc" id="classPrimitive_html_aa9c00e6dfba12ffc83523e010881ff8f"><div class="ttname"><a href="classPrimitive.html#aa9c00e6dfba12ffc83523e010881ff8f">Primitive::control</a></div><div class="ttdeci">Control::Control control() const</div><div class="ttdoc">Get the control indicator. </div><div class="ttdef"><b>Definition:</b> primitive.h:339</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_a70b6fa02c57c146aa26eb85abc085d2a"><div class="ttname"><a href="classMPL_1_1env__base.html#a70b6fa02c57c146aa26eb85abc085d2a">MPL::env_base::set_j_max</a></div><div class="ttdeci">void set_j_max(decimal_t j)</div><div class="ttdoc">Set max acc in each axis. </div><div class="ttdef"><b>Definition:</b> env_base.h:243</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_ac85f1da0ec8736ba5c379bbdb6634fe2"><div class="ttname"><a href="classMPL_1_1env__base.html#ac85f1da0ec8736ba5c379bbdb6634fe2">MPL::env_base::tol_pos_</a></div><div class="ttdeci">decimal_t tol_pos_</div><div class="ttdoc">tolerance of position for goal region, 0.5 is the default </div><div class="ttdef"><b>Definition:</b> env_base.h:374</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_ab2b77b8f234481dc74d0223a87dcff3c"><div class="ttname"><a href="classMPL_1_1env__base.html#ab2b77b8f234481dc74d0223a87dcff3c">MPL::env_base::set_v_max</a></div><div class="ttdeci">void set_v_max(decimal_t v)</div><div class="ttdoc">Set max vel in each axis. </div><div class="ttdef"><b>Definition:</b> env_base.h:237</div></div>
<div class="ttc" id="classMPL_1_1env__base_html_a9d1c12973cfed2b1ceea81664008ed55"><div class="ttname"><a href="classMPL_1_1env__base.html#a9d1c12973cfed2b1ceea81664008ed55">MPL::env_base::set_tol_acc</a></div><div class="ttdeci">void set_tol_acc(decimal_t acc)</div><div class="ttdoc">Set acceleration tolerance for goal region. </div><div class="ttdef"><b>Definition:</b> env_base.h:268</div></div>
</div><!-- fragment --></div><!-- contents -->
<!-- start footer part -->
<hr class="footer"/><address class="footer"><small>
Generated by &#160;<a href="http://www.doxygen.org/index.html">
<img class="footer" src="doxygen.png" alt="doxygen"/>
</a> 1.8.13
</small></address>
</body>
</html>
